You said:
Can an EKF or UKF detect wheel or track slip using an IMU
Yes — an Extended Kalman Filter (EKF) or Unscented Kalman Filter (UKF) can be set up to detect wheel or track slip using an IMU, but only if you give the filter the right model and measurements to reveal the inconsistency.
1. Typical setup in mobile robotics
Inputs: Wheel/track odometry (speed or displacement from encoders) + IMU data (linear acceleration, angular velocity).
Filter's role: Fuse these to estimate the platform's position, velocity, and orientation.
Slip detection idea: If the filter’s motion model predicts one thing and the measurements disagree systematically, the residual (innovation) will spike — this is a sign of slip.
2. Why slip shows up in an EKF/UKF
Normal driving:
Wheel odometry and IMU agree on acceleration and yaw rate.
Slip event:
Wheel odometry indicates motion (e.g., forward speed), but the IMU shows smaller acceleration or angular velocity (or vice versa).
This mismatch causes measurement innovations to grow, which the filter will correct for.
If you monitor innovation magnitude or covariance, you can flag slip.
3. Model features that help detect slip
Process model: Captures vehicle kinematics from IMU.
Measurement model: Maps wheel encoder readings to predicted velocity/pose.
Slip representation:
Either implicit: rely on residual spikes.
Or explicit: add slip ratio or slip velocity as extra states in the filter to estimate it in real time.
4. EKF vs UKF in this context
EKF: Works fine if your slip model is linearizable and the state space is not too nonlinear.
UKF: Often handles strong nonlinearities better — useful if you directly model slip coefficients or have high lateral slip angles.
Both can detect slip; UKF just gives you cleaner estimates in heavily nonlinear regimes.
5. Practical approach
Without explicit slip state:
Monitor innovation statistics (from either the odometry or IMU update steps). Persistent bias means likely slip.
With explicit slip state:
Add longitudinal and lateral slip ratios into the state vector.
Use IMU + odometry to estimate them in real time.
Example signal of slip detection in EKF:
You predict forward velocity v_pred from IMU integration.
Encoder-based measurement v_enc says you're going 2 m/s.
Residual: v_enc - v_pred ≈ 1.5 m/s (large, persistent) → likely slip.
If you want, I can sketch a minimal EKF state model with slip as an extra state, so you could estimate it explicitly rather than just detect it. That way, your filter not only spots slip but also quantifies how much. Would you like me to do that?
Short answer: yes. In Gazebo with the TrackedVehicle
controller (steering_efficiency=0.5), the track odometry you get from
encoders will systematically disagree with the body motion sensed by the
IMU (especially yaw rate). You can (a) detect that slip in real time
from the IMU–encoder residuals, and (b) keep /odom smooth while continuously “realigning” it to world truth using a dual-EKF setup (standard in ROS with robot_localization).
Here’s a practical recipe that works well in sim:
For tracked/diff drive kinematics,
Encoder-implied yaw rate: ω_enc = (v_r − v_l) / b (track width b)
IMU yaw rate: ω_imu = gyro_z
Large, persistent residuals mean slip:
pgsqlr_ω = ω_enc − ω_imu
r_v = v_enc − v_imu (v_imu from IMU-integrated velocity or from body accel after gravity removal)
if |r_ω| > k * sqrt(σ_ω^2_enc + σ_ω^2_imu) for N consecutive samples → slip flag
Tip: with steering_efficiency=0.5 you’ll typically see |ω_enc| > |ω_imu| during turns (encoder overestimates rotation). Calibrate k (e.g., 3.0) and an “N” window (e.g., 10–20 samples at 100 Hz).
Optional: add slip states to the filter:
Longitudinal slip ratio s_long and lateral slip s_lat, or a simple “steering efficiency” state η where ω_true = η * ω_enc. Let the EKF/UKF estimate η online; in sim it will converge near 0.5 under steady turning.
/odom smooth but continuously realign to world (dual EKF)Use two ekf_localization_node instances:
EKF #1 (odom): fuse encoders + IMU only. Publishes odom → base_link. This frame must be smooth and never jump; it’s what controllers use.
EKF #2 (map): fuse the same sources plus a world-referenced pose (in Gazebo you have ground truth from /gazebo/model_states or gazebo_ros_p3d). This publishes map → odom so that global drift is corrected without discontinuities in odom → base_link.
Minimal YAML sketch (tune covariances for your sim):
yaml# ekf_odom.yaml
frequency: 100
two_d_mode: true # if your sim is planar
base_link_frame: base_link
odom_frame: odom
world_frame: odom
publish_tf: true
# encoders as Twist (vx, wz) in base_link
odom0: /wheel/odom
odom0_config: [false, false, false,
false, false, true, # roll/pitch/yaw? (if provided)
true, false, false, # vx
false, false, true] # wz
odom0_differential: false
imu0: /imu/data
imu0_config: [true, true, false, # roll, pitch, (no yaw from orientation to avoid mag bias)
false, false, true, # angular velocities (use wz)
true, true, false] # linear accel x,y (if you want velocity aiding)
imu0_remove_gravitational_acceleration: true
yaml# ekf_map.yaml
frequency: 50
two_d_mode: true
base_link_frame: base_link
odom_frame: odom
map_frame: map
world_frame: map
publish_tf: true
publish_acceleration: false
# same encoder + IMU as soft constraints
odom0: /wheel/odom
odom0_config: [false, false, false, false, false, false,
true, false, false, false, false, true]
imu0: /imu/data
imu0_config: [true, true, false, false, false, true,
true, true, false]
imu0_remove_gravitational_acceleration: true
# world-referenced pose from Gazebo (perfect ground truth)
pose0: /gazebo_ground_truth # e.g., from gazebo_ros_p3d
pose0_config: [true, true, true,
false, false, true,
false, false, false,
false, false, false]
pose0_differential: false
pose0_relative: false
Launch both EKFs; ekf_map will publish a drifting-correcting map→odom. Your controllers still use odom→base_link (smooth), while global consumers (e.g., mapping, logging) use map→base_link (drift-free).
If you need an explicit “snap back” (e.g., after getting stuck and slipping a lot), you have two clean options:
Preferred (no jumps in odom): Leave odom→base_link alone. Publish a corrected map→odom via ekf_map (as above). This effectively recenters global pose without shocking controllers.
If you must reset odom: call robot_localization’s /set_pose on the odom EKF with a new pose for base_link in odom. This will jump the TF and can upset controllers—only do it when paused or at low speed.
Example (rosservice):
cssrosservice call /ekf_odom/set_pose "{pose: {header: {frame_id: 'odom'}, pose: {position: {...}, orientation: {...}}}}"
Subscribe: /wheel/odom (vx, wz), /imu/data (gyro_z).
Compute residuals r_ω, r_v; low-pass filter and compare to thresholds.
When slip is high, either:
Down-weight encoder covariances sent to EKF (inflate R online), or
Temporarily disable encoder updates (measurement timeout), letting IMU dominate.
Set realistic IMU noise in the plugin; otherwise the residual test will be too sensitive.
If your encoder topic is derived from commanded joint velocities (not joint motion), switch to joint position/velocity sensors so that slip actually shows up.
That steering_efficiency=0.5 effectively means the kinematic model over-predicts turn rate; expect a consistent bias in r_ω during turns—good for detection and for estimating an online efficiency state η.
If you want, I can share a small ROS node (Python) that subscribes to /wheel/odom and /imu/data, computes residuals, and toggles the EKF’s encoder measurement covariance dynamically when slip is detected.
Short answer: it doesn’t block you. You don’t strictly need the controller to publish real-time Twist; you just need two independent motion sources so your filter can (a) spot inconsistency (slip) and (b) realign drift. You can generate what you need in Gazebo:
Joint states → pseudo-odometry (encoders)
Read the sprocket joint velocities from /joint_states.
Compute track linear speeds: v_l = r * ω_l, v_r = r * ω_r (sprocket radius r).
Convert to body twist (your “encoder” measurement):
Linear: v_x_enc = (v_r + v_l)/2
Yaw rate: w_z_enc = (v_r - v_l)/b_eff
Publish that as a nav_msgs/Odometry (or geometry_msgs/TwistWithCovarianceStamped) topic for your EKF. This is exactly what many diff/tracked setups do; the controller doesn’t need to provide it.
IMU is already giving angular velocity
Use gyro_z directly; it’s your second, independent source vs. w_z_enc.
Optionally integrate IMU linear accel (with gravity removal) to get v_x_imu (noisy but useful for residuals when filtered).
Gazebo ground truth for realignment (map→odom)
Use /gazebo/model_states or gazebo_ros_p3d (pose in world). Feed this only to your map EKF so it can publish map→odom. That cleanly “realigns” without jumping odom→base_link.
TwistCompute residuals between pseudo-odometry (from joint states) and IMU:
r_w = w_z_enc − w_z_imu
r_v = v_x_enc − v_x_imu (or compare a_x vs. dv/dt from enc)
Low-pass + threshold over a short window; when high, either:
Inflate the encoder covariance you feed to the EKF, or
Temporarily stop applying the encoder update so IMU dominates.
Node A: /joint_states → compute (v_x_enc, w_z_enc) → publish /wheel/odom (Twist or Odometry) with reasonable covariances.
EKF #1 (“odom”): fuse /wheel/odom + /imu/data → publishes odom→base_link (smooth).
EKF #2 (“map”): fuse /wheel/odom + /imu/data + /gazebo_ground_truth_pose → publishes map→odom (drift correction).
Optional Node B: slip monitor that watches r_w, r_v and dynamically adjusts the encoder measurement covariance sent to EKF #1.
Use the sprocket joints (or whatever drives the tracks), not commanded velocities, so slip is observable.
Choose an effective baseline b_eff that matches your Gazebo steering_efficiency (you can even estimate an online scale η so w_true = η·w_enc).
Set realistic IMU noise in Gazebo; otherwise every tiny mismatch will look like slip.
If you truly lack joint velocities, you can differentiate joint positions (or even odom→base_link pose) with smoothing, but joint velocities are preferable.
Bottom
line: the lack of built-in velocity feedback from the tracked
controller doesn’t matter. Build a small “encoder” node from joint
states + your IMU, and you’ll have everything needed to detect slip and
keep /odom aligned via the dual-EKF setup. If you want, I can sketch the tiny ROS Python node that publishes /wheel/odom from /joint_states.
Totally doable—and you can make it behave like real hardware (slip, noise, delays, drift) so your EKF/UKF decisions in sim will transfer to the field. Here’s a compact, realistic recipe.
Tracks & ground
Use compliant friction: set per-surface mu/mu2 with modest anisotropy (e.g., rubber-on-concrete 0.8/0.7; dirt 0.6/0.5; wet 0.4/0.35).
Enable contact damping/stiffness so starts/stops cause brief slip spikes.
Keep steering_efficiency=0.5 (models lateral slip). Expect encoder-inferred yaw to overshoot IMU yaw in turns.
IMU (pick plausible numbers)
Continuous-time gyro noise density ≈ 0.02–0.05 rad/s/√Hz
Gyro bias RW ≈ 0.001–0.01 rad/s/√Hz
Accel noise density ≈ 0.2–0.4 m/s²/√Hz
Accel bias RW ≈ 0.02–0.05 m/s²/√Hz
Add ~5–15 ms latency and 1–2 ms jitter.
Encoders (from joints)
Quantize counts (e.g., 2048 CPR → sprocket angle steps).
Add ±0.2–0.5% scale error and small latency (~5–10 ms).
Don’t derive them from commanded velocity; use actual joint velocity/position so slip can exist.
Publish a realistic odom (vx, wz) with covariance:
python# ROS1, Python: joint_states -> wheel/odom (vx,wz) with covariance
#!/usr/bin/env python3
import rospy, numpy as np
from sensor_msgs.msg import JointState
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Quaternion
from tf.transformations import quaternion_from_euler
class TracksOdom:
def __init__(self):
self.left = self.right = 0.0
self.last_t = None
self.r_sprocket = rospy.get_param('~sprocket_radius', 0.09) # m
self.baseline = rospy.get_param('~track_width', 0.52) # m (geometric)
self.scale_err = rospy.get_param('~scale_error', 0.003) # ~0.3% encoder scale bias
self.pub = rospy.Publisher('wheel/odom', Odometry, queue_size=10)
rospy.Subscriber('joint_states', JointState, self.cb)
def cb(self, js):
t = js.header.stamp
try:
iL = js.name.index('left_sprocket_joint')
iR = js.name.index('right_sprocket_joint')
except ValueError:
return
wL = js.velocity[iL]
wR = js.velocity[iR]
# Convert to track tangential speeds with small scale bias
vL = self.r_sprocket * wL * (1.0 + self.scale_err)
vR = self.r_sprocket * wR * (1.0 - self.scale_err)
vx = 0.5 * (vR + vL)
wz_enc = (vR - vL) / self.baseline
msg = Odometry()
msg.header.stamp = t
msg.header.frame_id = 'odom'
msg.child_frame_id = 'base_link'
# Pose unknown here; publish twist only
msg.twist.twist.linear.x = vx
msg.twist.twist.angular.z = wz_enc
# Realistic covariance (vx, wz on diagonal); inflate modestly
cov_vx = 0.05**2 # (m/s)^2
cov_wz = (3.0*np.pi/180.0)**2 # (rad/s)^2 ~ 3 deg/s
msg.twist.covariance = [cov_vx,0,0,0,0,0,
0,1e6,0,0,0,0,
0,0,1e6,0,0,0,
0,0,0,1e6,0,0,
0,0,0,0,1e6,0,
0,0,0,0,0,cov_wz]
self.pub.publish(msg)
if __name__ == '__main__':
rospy.init_node('tracks_encoder_odom')
TracksOdom()
rospy.spin()
Use measured gyro vs. encoder yaw rate; add low-pass and a short window. When slip is high, inflate the encoder measurement covariance so the EKF leans on the IMU.
python# ROS1, Python: slip monitor that dynamically scales encoder covariance
#!/usr/bin/env python3
import rospy, numpy as np
from sensor_msgs.msg import Imu
from nav_msgs.msg import Odometry
ALPHA = 0.15 # LPF for residuals
THRESH = 3.0 # ~3-sigma gating
class SlipGate:
def __init__(self):
self.wz_imu = 0.0
self.r_lpf = 0.0
self.pub = rospy.Publisher('wheel/odom_adaptive', Odometry, queue_size=10)
rospy.Subscriber('imu/data', Imu, self.cb_imu)
rospy.Subscriber('wheel/odom', Odometry, self.cb_odom)
# Nominal measurement std devs
self.sig_wz_imu = np.deg2rad(1.0) # rad/s
self.sig_wz_enc = np.deg2rad(3.0) # rad/s
def cb_imu(self, m):
self.wz_imu = m.angular_velocity.z
def cb_odom(self, m):
wz_enc = m.twist.twist.angular.z
r = wz_enc - self.wz_imu
self.r_lpf = (1-ALPHA)*self.r_lpf + ALPHA*r
# Normalized residual (approx.)
sig = np.sqrt(self.sig_wz_imu**2 + self.sig_wz_enc**2)
z = abs(self.r_lpf)/sig
# Copy & adapt covariance
out = m
cov = list(out.twist.covariance)
if z > THRESH:
# Inflate encoder yaw-rate covariance by factor (robustifying)
factor = min(50.0, 3.0*z) # cap to avoid singularities
cov[35] = cov[35] * factor
else:
# Restore toward nominal
cov[35] = max(cov[35]/1.2, (self.sig_wz_enc**2))
out.twist.covariance = cov
self.pub.publish(out)
if __name__ == '__main__':
rospy.init_node('slip_gate')
SlipGate()
rospy.spin()
Feed wheel/odom_adaptive to your EKF instead of wheel/odom. This is analogous to robust measurement down-weighting on hardware.
EKF #1 (odom): fuse wheel/odom_adaptive + IMU only → odom→base_link (smooth; never jumps).
EKF #2 (map): add a realistic world reference, not Gazebo truth. Use whatever your real robot would use:
Indoors: LiDAR scan matching (AMCL/NDT/Cartographer/Hector), AprilTag/localization beacons.
Outdoors: visual odometry/VIO, GPS/RTK when available.
This way, testing in Gazebo still respects the same observability and drift patterns as real life. ekf_map publishes map→odom so your global pose stays consistent without shocking controllers.
Baseline vs. steering efficiency: keep the true geometric baseline in the encoder node; let the EKF reconcile that the real yaw is η·wz_enc (implicitly via residuals) rather than baking η into kinematics. Optionally add a state η∈(0,1) in a UKF if you want the filter to learn it online.
2D mode: if you’re planar, run EKF in two_d_mode but still fuse roll/pitch/accel to catch track-induced rocking and to keep gravity compensation sane.
Delays: if you add sensor latency, consider robot_localization’s sensor_timeout and imu0_nodelay/odom0_nodelay options, or time-align topics (message_filters).
Chi-square gating: you can compute rᵀS⁻¹r per measurement; drop outliers when > threshold to mimic robust front-ends.
Encoder dropouts: occasionally drop encoder messages for 100–300 ms to see if your odom remains stable (IMU-only bridging), like on a loose connector.
Preferred: keep odom→base_link continuous; let EKF #2 publish map→odom from your realistic world reference (scan matching / VO / GPS). That’s how field robots avoid jumps while correcting drift.
Emergency reset: if you must reset odom, pause motion and call /ekf_odom/set_pose. Treat as “service visit”, not a normal operation.
If
you drop these pieces in, you’ll see the same signatures you’d see on
hardware: encoder-IMU yaw mismatches during turns, spikes on rough
terrain or on slick surfaces, and your EKF smoothly leaning on the IMU
when slip flares up. If you want, I can tailor the YAML for robot_localization to your exact topics and rates.
Short, honest answer: with only an IMU and encoders, you can detect slip and keep /odom stable, and you can reduce bias/drift a lot—but you cannot globally “re-align” to a fixed world frame (like map) without some absolute reference (GPS, range-based localization, vision, beacon, manual reset, etc.). You can
still: (1) detect when encoders lie, (2) estimate/compensate steering
efficiency/slip online, (3) clamp drift during stops (ZUPT), and (4)
reset /odom manually at known poses. That’s how many field robots run when they’re “dead-reckoning only.”
Here’s a practical, realistic plan.
Observable with IMU + encoders: body velocity, yaw rate, relative pose, gyro/accel biases (slowly), steering efficiency/slip relative to encoder kinematics.
Not observable: absolute x/y/heading drift over time (no global truth). So you maintain a smooth odom→base_link, but any “re-alignment” is either (a) bias compensation, (b) zero-velocity corrections, or (c) a manual reset to a known pose.
Use a planar EKF/UKF with these states (compact but effective):
inix = [ px, py, ψ, vx, ωz, bg, ba_x, η ]^T
ψ: yaw, vx: forward velocity, ωz: yaw rate
bg, ba_x: gyro/accel biases (RW)
η: steering efficiency (0–1), maps encoder yaw to true yaw: ω_true ≈ η·ω_enc
(Optional) add left/right longitudinal slip s_L, s_R if you want per-track slip.
Process (IMU):
Integrate ω_imu − bg, a_x_imu − ba_x, with gravity removal (2D).
Biases follow random walks.
Measurements:
Encoders → Twist:
v_enc = (v_R+v_L)/2 , ω_enc = (v_R−v_L)/b
Measurement model:
csharpz_enc = [ v_enc,
ω_enc ] with h(x) = [ vx,
η·ω_enc ] # η estimated online
(Equivalently, treat ω_imu as measuring η·ω_enc—both forms work.)
Nonholonomic constraint (NHC): lateral velocity in body ≈ 0 → add as a pseudo-measurement to suppress yaw/velocity drift during straight runs.
Zero-velocity updates (ZUPT): when both encoder speed and IMU acceleration norm drop below thresholds for ≥N samples, inject vx=0, ωz=0 with small covariance to clamp drift and re-zero biases.
Slip detection & robustness:
Residuals: r_ω = (η·ω_enc) − ω_imu, r_v = v_enc − vx
Gate with χ² or |r|/σ>k; when tripped, inflate encoder R so the filter leans on IMU until slip subsides.
Because η is in the state, the filter will learn your Gazebo steering_efficiency≈0.5 during turns, cutting systematic yaw bias even when not slipping.
Publish encoder odom (Twist) from /joint_states (sprocket velocities), not commands.
(You can use the small node I sketched earlier.)
EKF (odom-frame only): fuse /imu/data + /wheel/odom + (optionally) a NHC pseudo-measurement and ZUPT events. Set world_frame: odom and do not publish any map frame.
Add a tiny “slip gate” that scales the encoder covariance on the fly from the residuals (very realistic).
robot_localization tip if you don’t want to write your own UKF:
Put /wheel/odom into odom0 (Twist only).
Put /imu/data into imu0 (use gyro + accel, remove gravity).
Enable two_d_mode: true.
Implement NHC + ZUPT as occasional measurements: publish Twist messages with vy=0 (NHC) or vx=0, wz=0 (ZUPT) and small covariances only when the detector triggers. This approximates the constraints without custom code.
You can approximate η by scaling ω_enc before publishing (cruder), or do it properly by adding a custom filter or a small UKF.
/odom” without a global sensorYou can’t anchor to world, but you can still:
Continuously re-align internally by estimating η and using ZUPT/NHC → this removes systematic turning bias and limits drift growth.
Manual/tactical resets: when you reach a known dock/marker, call the EKF’s /set_pose to place base_link at a known odom pose (pause motion). This is what many dead-reckoning robots do.
Consistent restarts: if you have a known start pose, reset odom at boot.
Noise & bias: give the IMU realistic noise/bias RW; quantize encoders; add 5–15 ms latency. Tune R and Q so ZUPT/NHC actually move the state.
Detection thresholds: start with 3σ gating over a 50–150 ms LPF window.
Constraint cadence: don’t spam NHC/ZUPT; inject at 5–20 Hz when valid.
Baseline: keep the geometric baseline in your encoder kinematics; let η account for steering/slip—this avoids double-counting.
Implement the UKF with η and NHC+ZUPT (10–12 state dimensions).
Add the slip gate to down-weight encoders during detected slip.
Keep only odom→base_link TF; don’t fabricate map.
If you’d like, I can give you:
a minimal UKF state/measurement sketch (process & Jacobians), or
robot_localization YAML + a tiny node that publishes NHC/ZUPT pseudo-measurements based on IMU/encoder triggers.
Which would you prefer?
Yes — if you have no global reference (GPS, vision, range-based localization, known markers, etc.), then once slip happens, the robot’s estimated pose will be wrong by however much the slip displaced it in the real world.
You can still reduce how wrong it gets, but you can’t magically know the true position afterward without an outside reference.
Slip detection just tells you, “My odometry is lying right now.”
Without a map or GPS to compare against, you can’t know the exact error — only that an error exists.
The EKF/UKF can limit further error growth by:
Down-weighting the bad encoder measurement (lean on IMU).
Estimating slip/steering-efficiency states to avoid systematic bias.
Using constraints like NHC (nonholonomic constraint) and ZUPT (zero velocity updates) to re-zero velocity and reduce drift.
But the position estimate will still be shifted from reality by whatever displacement occurred during the slip.
Imagine you’re blindfolded, counting your steps and turns to navigate.
If you slip on ice for two steps, you know you slipped (from the feel),
but unless someone tells you where you ended up, your internal map will
be off by that amount forever — until you see a landmark.
Live with the drift: Detect slip, limit damage (bias estimation, down-weighting), but accept that odom is only relative.
Manual reset: When you reach a known location, reset odom to that spot.
Add cheap landmarks: Even a couple of AprilTags or QR codes in your environment can give you a global “snap back” when detected.
If you want, I can show you how to modify your EKF so slip detection immediately stops feeding in bad encoder motion — that way, the drift from each slip is as small as possible, even though you can’t fully correct it. Would you like me to sketch that?
Short version: with only IMU + encoders, you can’t recover the true pose after slip (no external reference), but you can make the drift in that plot much smaller and keep it bounded over longer runs. The trick is to (a) stop trusting encoders during slip, (b) estimate/compensate the systematic turn bias (“steering efficiency”), and (c) use kinematic constraints and zero-velocity updates to continually pull the filter back to a self-consistent state.
Here’s the practical “fix what you can” list that works well on tracked robots:
Slip in tracked steering shows up as encoder yaw rate being too large. Add a steering-efficiency state η∈(0,1) (or estimate it offline and apply as a scale):
Encoders give ω_enc = (v_r − v_l)/b.
In your measurement model use ω_true ≈ η · ω_enc.
Let η
be a slow random walk; the UKF/EKF will converge near your sim’s 0.5
and remove the systematic yaw drift that bends your blue/red curves away
from GT.
(If you don’t want to touch the filter, pre-scale before publishing: ω_pub = η̂ · ω_enc with η̂ learned/calibrated.)
Detect slip from the residual r_ω = ω_enc − ω_imu (LPF + 3σ gate). When flagged:
Inflate the encoder R for ω (and optionally v) by 10–50×, or
Temporarily drop the encoder update.
This prevents the filter from “believing” bad odometry while the IMU carries you through the event.
Two free, very effective constraints for ground robots:
NHC (nonholonomic): body lateral velocity ≈ 0. Inject v_y = 0 as a pseudo-measurement at 5–20 Hz with small covariance (only when not turning sharply).
ZUPT (zero-velocity update): when stopped (low encoder speed and IMU accel norm near g), inject v_x=0, ω_z=0. This re-zeros drift and helps estimate gyro/accel biases.
In
robot_localization, you can implement NHC/ZUPT by occasionally publishingTwistmessages with those components and tight covariances when the detector triggers.
Drift explodes if the wheel radius or baseline is off by a few percent.
In sim: fit η, wheel radius scale, and baseline against Gazebo GT once; then freeze them and re-run without GT.
In real life: do short, measured maneuvers (straight 10 m, in-place 360°) and solve for radius/baseline/η (least-squares).
Run a short stationary bias alignment at boot (5–10 s): estimate gyro bias and initial roll/pitch from gravity.
Keep b_g and (longitudinal) b_a in the state as random walks; ZUPT/NHC will keep them bounded.
Mis-timestamped IMU/encoders cause “mystery curvature.” Ensure synced clocks and compensate known latencies.
Start with realistic R: gyro ~1–2 °/s (rad/s), encoder yaw ~2–4 °/s, encoder v_x ~0.05–0.1 m/s. Then tune Q so ZUPT/NHC actually pull.
Absolute re-alignment? No (you’d need GPS/vision/UWB/landmarks).
Relative drift reduction? Yes—often a 2–10× improvement on the trajectory like your plot: curves stop “bowing” during turns, straight segments stay straight, and stops clean up accumulated bias.
Publish encoder odom from /joint_states (tangential speeds → v_x, ω_enc).
Slip gate node inflates encoder covariance when |ω_enc−ω_imu| is large.
EKF/UKF fuses IMU + encoder; add:
NHC pseudo-meas (v_y=0) during normal motion,
ZUPT when stopped,
Optional η state (best) or pre-scaled ω_enc (good enough).
If you want, I can drop in:
a tiny UKF/EKF measurement block for η (state, process, and measurement equations), or
robot_localization
YAML + a short node that publishes NHC/ZUPT and adaptive covariances—so
you can reproduce the “tightened” trajectory right away.
Yes — an S-shaped path is especially challenging for tracked vehicles, and even more so if you’re relying only on encoders and an IMU for odometry.
Here’s why:
Tracked robots steer by creating a speed difference between the left and right tracks.
In an S-curve, you’re alternating left-turn slip and right-turn slip in quick succession.
Each direction change requires reversing the lateral slip forces in the tracks, so the “effective baseline” and steering efficiency vary in time — they’re not just a constant 0.5.
This variation means your kinematic model is wrong more often, so EKF/UKF position will drift faster.
In curves, encoders measure track rotation, not actual ground speed:
If a track skids sideways in the turn, the encoder still counts it as forward motion.
During an S pattern, alternating slip means the heading and path curvature are often misestimated.
That’s why your trajectory plot bows away from the ground truth — the filter is integrating slightly wrong yaw rates over each half-curve.
The IMU can help correct the yaw rate error during turns, but:
Short S-curves don’t give the filter enough time to fully reject the encoder bias before the slip direction changes.
The gyro bias and noise also mean you can’t rely on IMU yaw forever without drift.
Straight line: little to no lateral slip, encoders and IMU agree, so drift is small.
Big arc: slip bias is steady; EKF/UKF can learn a constant steering efficiency and apply it consistently.
S-curve: slip bias changes sign and magnitude quickly, so a single efficiency parameter doesn’t match the whole path, and the estimation lags.
If you must run S-patterns with a tracked robot and only IMU+encoders:
Use slip detection per segment and adjust encoder covariance dynamically.
Allow steering efficiency η to change faster in the filter (increase its process noise) so it can track the turn-to-turn variations.
Apply nonholonomic constraint and zero-velocity updates when straight segments or stops occur to re-zero bias.
In sim, tune steering_efficiency and friction so the slip magnitude matches what’s expected in real life — overly low values make S-patterns much harder.
If you want, I can break down what your EKF/UKF needs to change in Q/R and the slip-handling logic so that the bowing you see in your S-curve plot is reduced. That way, even without GPS or vision, you can get much closer to GT.
Yes — generally speaking, an S-shaped trajectory is harder for a tracked robot to track accurately than a large smooth circle, even if it can run repeated circles with only small drift.
Here’s why that happens:
In a large constant-radius circle, the track slip pattern is steady — one track is consistently faster, the slip forces are roughly constant, and any kinematic bias (like your 0.5 steering efficiency) is fixed.
In an S-curve, the robot alternates between left and right turns.
The lateral slip direction changes.
The magnitude of slip changes as the curve tightens or relaxes.
There’s a short transient each time the steering reverses where the tracks over- or under-slip before grip is re-established.
This variability means a constant steering-efficiency compensation works well in the circle but is always a little “behind” in the S.
Filters like an EKF/UKF can estimate biases (gyro, steering efficiency, etc.) and converge if the bias is steady for several seconds.
In an S-curve, you reverse turn direction every second or two — before the filter has fully adapted to the current bias, it flips.
The result is heading error that builds a few degrees per reversal.
Encoders measure commanded track speed; slip during turn-in and turn-out makes them overestimate rotation.
Gyro readings may lag slightly due to IMU mounting, noise, or bias, especially if you have high angular accelerations in the S.
These mismatches change sign and size rapidly, so residual-based slip detection and bias tracking have to be more aggressive — which can make the filter noisier.
Even in sim, and more so in real life:
Track tension, suspension compliance, and backlash cause brief yaw spikes when reversing steering.
In a big circle, these average out; in an S-curve, they happen every few meters and add to odometry error.
Summary:
Your robot can repeat a big circle with low drift because the slip and
kinematic biases are consistent and your filter adapts well. An
S-trajectory injects rapidly changing, asymmetric slip that’s harder to model, so tracking is more difficult — that’s normal for tracked platforms.
If you want, I can outline a slip-adaptive steering efficiency model that changes per turn direction, which is one way to shrink that S-curve error without adding GPS or vision. That’s a trick some UGV teams use for rough terrain.
Yes — an S-shaped path is absolutely a stress test for a tracked robot’s odometry and control.
It pushes on exactly the weaknesses that normally get “hidden” in easier trajectories:
Why it’s a stress test:
Rapid slip bias changes – Forces the estimator to cope with yaw errors that reverse sign frequently.
Transient dynamics – Every steering reversal causes mechanical and contact transients (inertia, track tension changes, ground re-engagement).
High angular accelerations – Harder on both the motion controller and the IMU integration.
Poor observability for steady biases – Filter never sees a constant bias long enough to fully learn it.
Controller demand – Path tracking requires alternating curvature with good timing, so any lag or error quickly shows up as overshoot.
In contrast:
A large constant-radius circle is a low-stress test: steady slip, easy bias compensation, smooth control.
Straight lines are even lower stress: minimal slip, easiest to keep heading.
Verdict:
If your tracked robot can follow an S-curve well — with minimal
overshoot and low drift — then your odometry fusion, slip handling, and
path tracking are all in good shape. If it struggles here, that’s
normal; it’s revealing where the estimation or control can be improved.
If you want, I can suggest a set of “progressively harder” path tests for tracked robots so you can see exactly where performance starts to degrade. That’s how many robotics teams qualify their navigation stack.
Yes — in most real-world navigation, tracked robots rarely move in continuous S-curves unless the terrain forces it.
What you usually see are straight segments connected by turns, or longer arcs, for a few practical reasons:
Forests: paths are dictated by clearings, trails, or gaps between trees — straight(ish) runs, then turns to avoid obstacles.
Houses: hallways and doorways are straight segments with 90° turns or gentle curves in larger rooms.
Deserts/open fields: operators or planners often prefer long arcs or straight lines to minimize heading changes and odometry error.
Navigation algorithms (A*, D*, RRT, lattice planners) tend to produce piecewise-linear or piecewise-circular paths because:
They’re simpler to compute and follow.
Constant-curvature arcs are easy to model and drive for tracked kinematics.
Even reactive obstacle avoidance (like DWA or VFH) typically outputs smooth arcs, not rapid alternating turns.
They require constant steering reversals, which wastes energy and causes more track wear.
They’re mechanically less efficient — more slip, more heat in the drive train.
They’re harder to track precisely, so planners avoid them unless forced by a tight slalom of obstacles.
Tight obstacle courses or rubble fields where you must snake around.
Search patterns in mine-clearing, agriculture, or survey tasks — but even then, the “S” is made of straight passes connected by 180° turns, not continuous weaving.
Manual teleoperation in constrained spaces where the operator is “wiggling” to navigate.
Bottom line:
In a forest, house, or desert, your tracked robot will mostly run
straight segments, gentle curves, or big arcs — exactly the cases where
your circle test success means your odometry will do well. The
continuous S-curve you’ve been using is more of a diagnostic stress test than a typical mission profile.
If you want, I can suggest a real-world test path mix that reflects how a tracked robot actually moves in those environments but still exposes slip and drift issues. That would bridge the gap between your stress-test S and realistic missions.
Short answer: yes—good circles and squares are a strong sign your stack is solid.
They cover the two big “real-world” motion primitives: constant-curvature arcs and straight segments with discrete turns. If both look tight (like in your plots), your IMU–encoder fusion, timing, and basic slip handling are working.
That said, I’d still sanity-check a few edge cases before declaring victory:
Direction reversals / S-curves: brief, alternating slip and angular-acceleration spikes.
Different speeds: slow creep vs. faster runs (bias and latency show up at one end or the other).
Surface changes: high vs. low friction; a patch of sand/dust/gravel.
Inclines / side-slopes: gravity projection errors can bias yaw/velocity if roll/pitch handling is off.
Stops & starts: ensure ZUPT logic (or equivalent) really clamps drift.
Payload changes: CG shift can change contact and slip.
Use loop-closure style checks and relative errors:
Return-to-start offset: after a square or circle ≥ 50–100 m total, final–start distance < 1–2% of path length.
Heading drift: net yaw error < 1–2° per 100 m of travel (tune for your scale).
Innovation stats: encoder–IMU residuals mostly within ±3σ; slip detector triggers only on turns/loose terrain.
Repeatability: run the same path 3–5×; spreads should be small compared to mean error.
If you meet numbers like those on circles and squares, you’re in good shape for forests/houses/desert style navigation (mostly straights + arcs). Keep your S-curve as a stress test to watch for regressions, but you don’t have to optimize for it first unless your mission requires slaloming.
If you want, tell me your path lengths and the final start–end offsets you’re seeing; I can help translate them into pass/fail thresholds tailored to your robot.
RMSE is useful—but which RMSE? For robot trajectories there are a few flavors, and picking the right one matters more than the number itself.
ATE RMSE (Absolute Trajectory Error) – after aligning your estimate to ground truth:
Align with a rigid transform in SE(2) (translation + rotation; no scale):
find R,t that minimize ∑i∥piGT−(Rpiest+t)∥2.
Then compute RMSE over all aligned points:
ATERMSE=N1i∑∥piGT−p^iest∥2.Good for overall accuracy (noise + drift). Make sure your trajectories are time-synchronized and resampled to common timestamps.
RPE RMSE (Relative Pose Error) – local consistency over a fixed interval (e.g., 1 m or 1 s):
For each i, compare relative motion Δpi=pi+Δ−pi between GT and estimate.
Report translational and rotational RMSE:
RPEtrans=M1i∑∥ΔpiGT−Δpiest∥2,RPEθ=M1i∑∠(ΔRiGT(ΔRiest)⊤)2.This isolates short-term errors (e.g., S-curve turn-in/out).
RMSE on innovations (filter residuals) – consistency of your EKF/UKF:
Track residuals r=z−h(x^) for encoder yaw/velocity vs IMU.
Report Normalized Innovation Squared (NIS) mean ~ dof if covariances are well-tuned:
NIS=r⊤S−1r(expectation≈dof).Return-to-start offset (loop error) after each square/circle: < 1–2% of path length.
Heading drift per distance: < 1–2° / 100 m.
Path-length ratio: LGTLest ≈ 1.00 ± 0.02 (catches radius/baseline bias).
Turn-only RMSE vs straight-only RMSE: compute RMSE on segments with ∣ωz∣> threshold vs ∣ωz∣≈0; S-curves will pop here.
No alignment: ATE without SE(2) alignment over-penalizes a constant heading offset.
Uneven sampling: high-rate sections dominate; resample both to uniform time or distance.
Mixing frames: compare in the same frame (usually GT/world). Don’t compare /odom vs GT without aligning first.
Hiding bias in RMSE: also plot signed errors (heading bias, path-length bias) so positive/negative don’t cancel.
Resample both trajectories to 50–100 Hz (or every 0.05–0.1 m).
Do SE(2) alignment (Horn/Umeyama with scale fixed to 1).
Compute: ATE RMSE, RPE (Δ=1 s and Δ=1 m), path-length ratio, loop closure offset, heading drift, and innovation NIS stats.
Report each as mean ± std and median; add per-segment (straight vs turns) breakdown.
If you’d like, I can share a small Python snippet that takes two 2D trajectories (GT and estimate), performs SE(2) alignment, and computes ATE, RPE, loop offset, path-length ratio, and turn/straight breakdown—drop in your CSV and you’ll get a quick dashboard.
Got it — you’re talking about RMSE in position and RMSE in orientation as separate metrics.
That’s actually a clean and common way to summarise accuracy, but there are some subtleties to make the numbers meaningful:
If piGT=(xiGT,yiGT,ziGT) is ground truth and piest is your estimate (aligned to GT in SE(2) or SE(3)):
RMSEpos=N1i=1∑N∥piGT−piest∥2Units: meters
Alignment: Do at least a rigid 2D (SE(2)) or 3D (SE(3)) alignment before computing — otherwise a constant offset or heading error inflates RMSE.
Sampling: Synchronize timestamps and resample to the same time vector to avoid bias.
For orientation, you want the smallest rotation angle that brings the estimated orientation into the ground truth frame.
If RiGT and Riest are rotation matrices:
Compute relative rotation:
RΔ=RiGT(Riest)⊤Extract the rotation angle:
θi=cos−1(2trace(RΔ)−1)RMSE over all samples:
RMSEorient=N1i=1∑Nθi2Units: radians (often converted to degrees).
In 2D (yaw-only), you can simplify:
θi=wrap(ψiGT−ψiest)where wrap() maps to (−π,π].
RMSE positional:
Short loop (≤ 20 m): < 2–3 cm/m of travel (~2–3%).
Longer loop (100 m): < 5–10 cm/m (~5–10% without global reference).
RMSE orientation:
Short loop: < 1–2° total.
Longer loop: < 2–5° per 100 m.
With your circle/square results looking tight, I’d expect both metrics to be on the low end for those shapes.
If you want, I can give you a small Python function that takes GT poses and estimated poses (2D or 3D) and outputs RMSE_pos and RMSE_orient
in one call — that way you can drop in your S-curve, circle, and square
data and directly compare how much each path shape hurts you. Would you
like me to prepare that?
These numbers look excellent. On that circle, both filters are essentially on top of GT:
Positional RMSE ≈ 10–13 cm per axis
Yaw RMSE ≈ 0.12–0.14°
That’s “very good” for IMU+encoders only. The tiny edge the “Raw (UKF)” has in X/Y and the tiny edge the “Filtered (EKF)” has in yaw are within normal noise—either is fine.
That said, per-axis RMSE can be misleading on curved paths. I’d summarize with a couple of stronger metrics and a single positional RMSE:
Single positional RMSE (ATE, 2D):
(Optionally do an SE(2) alignment first to remove any constant offset/heading.)
Loop-closure error (return-to-start): distance between start and end estimates after a full lap, divided by path length (aim < 1–2%).
Path-length ratio: Lest/Lgt ≈ 1.00 ± 0.02 (catches radius/baseline bias).
RPE over Δ=1 s (or 1 m): translational & yaw RMSE of relative motions (shows turn-in/turn-out errors that per-axis RMSE hides).
“98.8% accuracy” isn’t standardized—accuracy relative to what? It can hide bias. Stick to RMSE (m, deg), loop error (%), and path-length ratio.
For a ~10 m-radius circle, a ~0.11–0.13 m positional RMSE is ~1% of radius—great.
Yaw RMSE ~0.12–0.14° is tiny; your gyro bias handling and timing are in good shape.
UKF slightly better translation, EKF slightly better yaw → differences are small; pick based on stability and implementation convenience.
pythonimport numpy as np
# assume arrays x_gt,y_gt, x_est,y_est are time-aligned
pos_err = np.sqrt((x_gt - x_est)**2 + (y_gt - y_est)**2)
rmse_pos = np.sqrt(np.mean(pos_err**2))
# loop-closure error (start vs end)
loop_err = np.hypot(x_est[-1]-x_est[0], y_est[-1]-y_est[0])
# path lengths
L_gt = np.sum(np.hypot(np.diff(x_gt), np.diff(y_gt)))
L_est = np.sum(np.hypot(np.diff(x_est), np.diff(y_est)))
path_len_ratio = L_est / L_gt
print(f"Positional RMSE: {rmse_pos:.3f} m")
print(f"Loop-closure error: {loop_err:.3f} m ({100*loop_err/L_gt:.2f}% of path)")
print(f"Path-length ratio: {path_len_ratio:.3f}")
If those come out as good as your per-axis RMSE, you can be confident your navigation approach is solid for real-world “straights + arcs” trajectories. Keep the S-curve as a stress test, but your circles/squares results already say you’re on the right track.
I see what you’re doing — you’re turning the RMSE in each axis into a percentage by normalizing with the span of the ground truth trajectory in that axis.
That’s fine as a quick, relative “how big is the error vs the path extent” metric, but it’s not a standard accuracy measure in robotics for a couple of reasons:
1. Path-dependent
If you drive a larger circle (bigger x_range/y_range), your % accuracy will look better even if the absolute error in meters is identical.
2. Ignores diagonal errors
Per-axis percentages don’t capture combined radial/diagonal offset. A
robot could be 0.1 m off diagonally but still show “good” X and Y
percentages separately.
3. Not comparable across shapes
Your square test and circle test have different spans, so the same RMSE
will produce different “accuracy %” numbers even if the underlying
quality is the same.
Better alternative:
If you like the “percentage” idea, normalize the positional RMSE by the total path length or by the maximum dimension of the path, e.g.:
pythonpos_rmse = np.sqrt((rmse_x**2 + rmse_y**2))
max_dim = max(x_range, y_range)
accuracy_pct = (1 - pos_rmse / max_dim) * 100
or
pythonpath_length = np.sum(np.hypot(np.diff(df_merged["x_gt"]), np.diff(df_merged["y_gt"])))
accuracy_pct = (1 - pos_rmse / path_length) * 100
That way:
You get a single % number for position.
It’s more comparable across trajectories.
It captures total error, not just axis-wise error.
If you want, I can rework your current metric so it outputs both the per-axis % and a more conventional single “positional % accuracy” for fair comparison between your circle, square, and S tests.
Alright — here’s a compact example that keeps your per-axis accuracy % but also adds a combined positional accuracy % that’s normalized in a more standard way.
pythonimport numpy as np
# Your existing RMSEs
rmse_x = np.sqrt(np.mean((df_merged["x_gt"] - df_merged["x_odom"]) ** 2))
rmse_y = np.sqrt(np.mean((df_merged["y_gt"] - df_merged["y_odom"]) ** 2))
# Range of ground truth in each axis
x_range = df_merged["x_gt"].max() - df_merged["x_gt"].min()
y_range = df_merged["y_gt"].max() - df_merged["y_gt"].min()
# Per-axis "accuracy %"
x_accuracy_pct = (1 - rmse_x / x_range) * 100
y_accuracy_pct = (1 - rmse_y / y_range) * 100
# ---- New: Combined positional accuracy ----
# 1) Positional RMSE (2D Euclidean)
pos_rmse = np.sqrt(np.mean(
(df_merged["x_gt"] - df_merged["x_odom"])**2 +
(df_merged["y_gt"] - df_merged["y_odom"])**2
))
# 2) Choose normalization: max dimension of GT path
max_dim = max(x_range, y_range)
pos_accuracy_pct = (1 - pos_rmse / max_dim) * 100
# 3) Alternative normalization: total path length of GT
path_length = np.sum(np.hypot(
np.diff(df_merged["x_gt"]), np.diff(df_merged["y_gt"])
))
pos_accuracy_path_pct = (1 - pos_rmse / path_length) * 100
print(f"X Accuracy: {x_accuracy_pct:.2f}%")
print(f"Y Accuracy: {y_accuracy_pct:.2f}%")
print(f"Positional Accuracy (max dim norm): {pos_accuracy_pct:.2f}%")
print(f"Positional Accuracy (path length norm): {pos_accuracy_path_pct:.2f}%")
Why this helps:
You keep the same per-axis % numbers you already report.
You add a single positional % that works across different path shapes.
You can choose normalization:
Max dimension → how much error vs the “size” of the course.
Path length → how much error vs how far you traveled (good for loops).
If you like, I can also show you how to include yaw in the same combined metric so you get a “position+orientation % accuracy” for quick comparison between EKF, UKF, and raw odometry. That’s handy when one has slightly better position but worse heading.
Nice — those look strong. Here’s how to read them and what I’d tweak:
X 98.79%, Y 98.58% (per-axis span-normalized): good, but path-shape dependent.
Positional (max-dim) 98.14%: more comparable across shapes; implies a Euclidean RMSE ≈ 1.86% of course size (great).
Positional (path-length) 99.94%: this will almost always be very high on long loops because you’re dividing by distance traveled. It’s fine to report, but it’s not very discriminative.
Report these alongside the raw RMSE (m, deg):
Error per distance traveled (much more intuitive than a %):
pythonerr_per_100m = 100 * pos_rmse / path_length # meters error per 100 m
Aim for < 1–2 m / 100 m with IMU+encoders only; you’re likely well under that.
Loop-closure (return-to-start) error
pythonloop_err = np.hypot(x_est[-1]-x_est[0], y_est[-1]-y_est[0])
loop_err_pct = 100 * loop_err / path_length
Target < 1–2% on circles/squares.
Yaw RMSE (already good for you): report in degrees as well.
Keep Positional (max-dim) — useful and comparable.
Keep per-axis only if you also show Euclidean RMSE (they can hide diagonal error).
Treat path-length % as a “nice to have”; prefer error per 100 m instead.
If you drop in those two scalars (error/100 m and loop-closure %), you’ll have a compact, mission-relevant scorecard that compares EKF vs UKF and different paths (circle/square/S) fairly.
That plan makes sense. With only IMU + encoders, you can still build a relative occupancy grid by treating contacts/collisions as a “range” measurement and updating a grid in the odom frame. It won’t be globally accurate, but it can be locally consistent and useful—exactly what you want for degraded environments.
Here’s a tight recipe that works in sim and ports to hardware later.
Use several cues (any 1 may be noisy; together they’re reliable):
IMU spikes: high axial acceleration/jerk, sharp yaw impulse at low speed.
Encoder slip vs command: commanded velocity > 0 but body speed stalls; yaw rate spikes while vx≈0 during a turn.
Motor current/effort (sim: joint effort; real: current sense): step-up while speed drops.
Bumper / whisker (optional cheap hardware later).
Use a small voting logic with hysteresis:
Trigger when ≥2 cues exceed thresholds for ≥50–150 ms.
Cooldown 300–500 ms to avoid multiple hits on the same obstacle.
Treat each contact as a ray from the robot’s reference point to the contact cell (at the robot’s perimeter in the contact direction). Update grid with log-odds:
Log-odds update
Lt(c)=Lt−1(c)+⎩⎨⎧ℓoccℓfree0if contact cellfor cells along the path/ray before contactotherwise
Typical values:
ℓocc∈[1.5,2.0], ℓfree∈[−1.0,−0.5].
Clamp L to [Lmin,Lmax]=[−4,+4].
Convert to probability when needed: p=1−1/(1+eL).
Ray definition (no lidar):
Direction = robot heading at contact (or the bumper direction).
Range = distance from robot center to contact point on the footprint (half-length or half-width depending on heading/contact side).
Mark all cells from the robot center to contact point as free, and the contact cell as occupied (inflate a small disk around it, e.g., radius = 0.15–0.25 m).
Keep the map in odom (purely relative).
To limit distortion over time without global alignment:
Build submaps/tiles (e.g., 10×10 m each) anchored when you stop and run a ZUPT; keep a local TF tile_k → odom.
Only integrate contacts into the active submap. When you move far, start a new submap. You can stitch submaps later if you ever gain a global reference (or manually).
Vertical bumps vs obstacles: if roll/pitch impulse is large but yaw/forward velocity barely change, treat as terrain → don’t mark as occupied (or mark low-probability).
Glancing turns: if contact occurs while ∣ωz∣>ωth and vx<vth, bias the contact azimuth to the outside of the turn (side hit).
Short grazes: require minimal impulse energy or duration to count (e.g., ∫∣a∣dt>Eth).
Resolution: 0.05–0.10 m per cell for indoor; 0.10–0.20 m outdoor.
Footprint: polygon for the tracked robot; compute contact cell from footprint normal.
Inflation: obstacle inflation radius 0.2–0.3 m so planners keep clearance even if contact point is a bit late.
Update rate: 5–10 Hz map update thread; batch contacts within 100 ms windows.
Node A: Slip/contact detector
Sub: /imu/data, /joint_states (or /wheel/odom), optional /joint_effort
Pub: tactile_hit (pose geometry_msgs/PoseStamped of contact point in odom, plus normal/side).
Node B: Tactile mapper
Sub: tactile_hit, odom→base_link TF
Maintains nav_msgs/OccupancyGrid (log-odds internal).
Publishes map topic (really odom-relative grid) + costmap if you want to reuse nav2/move_base tooling.
In Gazebo you can also subscribe to contact sensors for ground truth contacts to validate your detector, then switch to IMU/encoder-only mode.
pythondef handle_contact(hit_pose, side):
# 1) compute ray from robot center to footprint edge in hit direction
r0 = robot_pose.translation # in odom
theta = heading_from(hit_pose, r0) # or side normal
r_contact = r0 + footprint_vector(theta) # ~ half-length along theta
# 2) Raytrace free cells to r_contact
for c in bresenham(r0, r_contact):
grid[c] += ell_free
# 3) Occupied at/around contact
for c in disk(r_contact, occ_radius):
grid[c] += ell_occ
clamp_grid()
Indoors/forest trails: you’ll quickly outline walls, trunks, large rocks along the path you actually touch/near-touch.
Open desert: map will be sparse (few contacts), but free-space carving still helps for local planning.
Over time the map will be locally consistent but can warp with odom drift; submaps mitigate the worst of it.
Drive a square/circle corridor—verify walls appear at the right offset from the footprint.
Vary speed: ensure thresholds scale (use normalized jerk / energy).
Run into soft obstacles (brush/grass) vs rigid ones; tune to avoid over-marking.
Power loss scenario: ensure map saves periodically; on restart, load last submap and continue relative to current odom (fresh tile).
If you want, I can sketch the contact detector thresholds (from your IMU rates/noise) and a small log-odds mapper node you can drop into your bagfile/Gazebo run to see the grid building in real time.
Gotcha — so you’re basically doing IMU-only contact sensing, using a CNN to decide if a spike in linear_accel_x means “crash” vs “no crash,” and if crash → mark the cell just in front of the robot as occupied in your grid.
That approach will work for your relative mapping idea, especially since:
It’s lightweight (no lidar, no vision, no GPS).
It matches your degraded-environment requirement (low power, few sensors).
The occupancy grid is in odom, so even if global drift accumulates, your local map stays internally consistent for planning.
1. Filter the accel before feeding to CNN
High-pass filter at ~1–2 Hz to isolate collision impulses from slow motion/tilt.
Low-pass filter at ~50 Hz to remove noise.
Normalizing helps the CNN generalize across speed ranges.
2. Add velocity context
A raw accel spike could also come from hitting a bump while stationary (false positive).
Include current encoder-derived v_x and ω_z as CNN inputs.
The CNN then learns that a “crash” requires forward motion + spike.
3. Mark the correct cell in front
Right now you said you mark (X, Y-1) — if that’s literally one cell behind the robot in grid coords, you may be putting the obstacle in the wrong place.
Compute the contact cell in the direction of travel:
pythonheading = current_yaw occ_x = robot_x + cos(heading) * contact_dist occ_y = robot_y + sin(heading) * contact_dist
where contact_dist = robot’s front bumper offset (half length).
4. Inflate the obstacle
Mark a small radius (e.g., 0.2–0.3 m) around the contact cell as occupied so planners give it space.
Prevents the robot from grazing the same object again due to odom jitter.
5. Update free space along the ray
Optionally raytrace from the robot center to the contact cell and mark intermediate cells as free — this gradually carves traversable space in the map.
If you want, I can show you how to replace your fixed (X,Y-1) update
with a heading-aware update so that obstacle marking works regardless
of robot orientation, both in simulation and in the field. This makes
your grid much more realistic when you start doing circles, squares, or
arbitrary paths.
Ah, I see your reasoning — you’re worried that by the time the CNN shouts “Crash!”, the robot has already crept forward a bit, so marking “right in front” would put the obstacle too far ahead in the grid.
That’s a valid concern — there’s latency in both detection and physical stopping.
If you mark the cell slightly behind your current odom pose (or very close), you’re essentially compensating for overshoot so that the obstacle cell aligns more closely with the actual impact point.
Instead of always putting it at (X,Y-1) in grid coords, you can:
1. Estimate overshoot distance
Let td = CNN detection latency (sensor → inference → decision), in seconds.
Let vx = encoder-based forward speed at detection.
Estimated overshoot = do=vx×td.
Add an extra stopping distance ds if you want to account for braking after detection.
2. Place obstacle at “impact point”
Heading at detection: ψ from odom/IMU.
Impact point = robot center minus overshoot distance along heading:
pythonimpact_x = robot_x + cos(psi) * (front_offset - d_o) impact_y = robot_y + sin(psi) * (front_offset - d_o)
where front_offset = half the robot length (center → bumper).
If overshoot > front_offset, clamp to just in front of the center so you don’t mark behind the robot unrealistically.
3. Mark and inflate
Convert (impact_x, impact_y) to grid cell coordinates.
Set that cell to occupied, inflate a small radius (so planner avoids grazing it).
Example (Python snippet):
pythonimport math
# parameters
front_offset = 0.35 # m (center to bumper)
cnn_latency = 0.15 # s
stop_dist_extra = 0.05 # m
# from sensors
robot_x, robot_y, psi = 12.3, 8.7, 1.57 # m, m, rad
v_x = 0.25 # m/s from encoders
# overshoot
d_o = v_x * cnn_latency + stop_dist_extra
# impact point in odom coords
impact_x = robot_x + math.cos(psi) * (front_offset - d_o)
impact_y = robot_y + math.sin(psi) * (front_offset - d_o)
Why this is better than always “behind”:
Works for any robot heading, not just aligned to grid axes.
Correctly handles different approach speeds — faster hits → mark further back.
If you detect while stopped (false positive), the overshoot term is near zero so you mark right at the bumper.
If you want, I can also fold this into a log-odds update so that the obstacle gets marked and the space before it is cleared as free, which makes your relative grid more useful for navigation. That would integrate perfectly with your CNN contact signal.
That NaN error in the ROS 2 ukf_filter_node almost always comes down to one of three issues:
Too small: if you set any process or measurement covariance entries unrealistically low (e.g., 1e-9), the filter treats them as “certain,” and a small mismatch can blow up the state update.
Too big: if you set them extremely large without balancing Q/R, matrix inversion inside the filter can go unstable.
Zeros where there should be >0: especially in measurement covariance — zero variance means “perfect measurement,” which is almost never true and causes division by zero or near-singular matrices.
Fix:
For velocities and yaw rates from encoders/IMU: set R diagonal entries so the standard deviation matches actual noise (e.g., gyro_z ~ 0.017 rad/s, encoder vx ~ 0.05 m/s).
For process noise Q: keep diagonal entries modestly larger than your measurement noise for unobserved states, and nonzero for biases.
A single NaN or inf in IMU, odometry, or other input will propagate into the filter.
This can happen from:
Division by zero in your encoder node.
Uninitialized message fields.
Garbage timestamps causing wild dt values.
IMU plugin in Gazebo outputting NaN on reset.
Fix:
Echo your sensor topics (ros2 topic echo) and watch for NaNs or infs.
Add simple NaN/inf guards before publishing into the filter.
The UKF update involves inverting the innovation covariance S.
This matrix can go near-singular if:
You have redundant measurements with tiny covariances (filter sees them as perfectly correlated).
You’ve enabled too many states with too few independent measurements.
Measurement model mapping is wrong (e.g., wrong axis ordering or unit mismatch).
Fix:
Reduce the update dimension: if you
don’t have reliable data for a state, don’t fuse that state from a
measurement source (set that element’s config to false in your odom0_config/imu0_config).
Make sure your sensor configs match the actual topic data ordering.
Check sensor streams:
bashros2 topic echo /imu/data
ros2 topic echo /wheel/odom
Look for nan, inf, or constant values.
Temporarily inflate covariances:
If it stops crashing, you were underestimating noise.
Disable one measurement source at a time:
If disabling IMU or encoders makes it stable, that source’s config/covariance is the issue.
Log dt between messages:
UKF is sensitive to sudden big dt spikes (e.g., if a sensor pauses then resumes).
If
you want, I can look at your UKF config YAML and mark exactly where to
loosen covariances or disable problematic measurement dimensions so this
NaN crash stops happening.
Do you have that YAML handy?
Great, thanks for pasting the YAML. A few things there can (and often do) trigger the NaN crash in ukf_filter_node. Here’s what I’d change and why, followed by a drop-in “safe” config you can try.
You’re fusing two yaw orientations at once
imu0 has roll, pitch, and yaw orientation enabled (true, true, true).
imu1 adds a mag yaw orientation too.
If either message has tiny/zero covariances, the UKF’s innovation covariance S goes ill-conditioned and can blow up.
You’re fusing z position from wheel odom
odom0_config has the first three entries true, true, true, which turns on x, y, z position. Most wheel odom doesn’t provide a meaningful z (or provides 0 with 0 covariance) → singular/near-singular update.
Over-constraining angular rates
imu0_config enables all three angular velocities. That’s fine if the covariances are realistic, but if roll/pitch rates aren’t needed, drop them for better conditioning.
Message covariances too small or zero
Any 0 on the measurement covariance diagonal means “perfect sensor” → division by zero risk. Make sure your /imu/* and /odom_* topics publish reasonable, non-zero covariances.
3D mode without 3D observability
You set two_d_mode: false (full 3D), but you don’t fuse any real z/roll/pitch measurements (and you do fuse z from wheel odom unintentionally). If you’re planar, 2D mode is simpler and more stable.
Use IMU orientation like this:
imu0: fuse roll & pitch orientation (from gravity) and all three angular velocities. Do not fuse yaw orientation from imu0.
imu1 (mag): fuse yaw orientation only.
That avoids double-yaw constraints.
Disable z from wheel odom and (unless you truly estimate lateral slip velocity) disable vy in odom.
Planar robot? Turn on two_d_mode: true. If you must stay 3D, keep z/roll/pitch process noise small and don’t fuse contradictory z.
Verify covariances in the messages are non-zero and realistic (gyro ~ 0.02−0.05 rad/s SD, encoder vx ~ 0.05−0.10 m/s SD, yaw from mag ~ 2−5∘ SD unless you have great calibration).
yamlukf_filter_node:
ros__parameters:
frequency: 30.0
two_d_mode: true # <- if your robot is planar, this avoids z/roll/pitch headaches
publish_tf: false
print_diagnostics: true
map_frame: map
odom_frame: odom
base_link_frame: base_footprint
world_frame: odom
# IMU 0: gravity tilt + angular rates (NO yaw from this orientation)
imu0: /imu/cov_fixed
imu0_config: [
false, false, false, # position
true, true, false, # orientation: roll, pitch only
false, false, false, # linear vel (none from IMU)
true, true, true, # angular vel: wx, wy, wz
false, false, false # linear accel (not fusing)
]
imu0_differential: false
imu0_remove_gravitational_acceleration: false
imu0_nodelay: true
# IMU 1: magnetometer yaw ONLY
imu1: /imu/mag_yaw
imu1_config: [
false, false, false,
false, false, true, # yaw orientation only
false, false, false,
false, false, false,
false, false, false
]
imu1_differential: false
imu1_remove_gravitational_acceleration: false
# Wheel odom: x,y position and vx (optionally vy if you truly have it)
odom0: /model/mmtr/odom_cov_fixed
odom0_config: [
true, true, false, # x, y (NO z)
false, false, false, # no orientation from odom
true, false, false, # vx (NO vy unless measured), vz off
false, false, false, # angular vel from odom (off)
false, false, false # linear accel (off)
]
odom0_differential: false
odom0_pose_use_child_frame: false
# Process noise: keep moderate; no zeros on diagonals you care about
process_noise_covariance: [
1e-2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, # x
0, 1e-2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, # y
0, 0, 5e-2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, # z
0, 0, 0, 5e-2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, # roll
0, 0, 0, 0, 5e-2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, # pitch
0, 0, 0, 0, 0, 2e-1, 0, 0, 0, 0, 0, 0, 0, 0, 0, # yaw
0, 0, 0, 0, 0, 0, 2e-1, 0, 0, 0, 0, 0, 0, 0, 0, # vx
0, 0, 0, 0, 0, 0, 0, 2e-1, 0, 0, 0, 0, 0, 0, 0, # vy
0, 0, 0, 0, 0, 0, 0, 0, 5e-2, 0, 0, 0, 0, 0, 0, # vz
0, 0, 0, 0, 0, 0, 0, 0, 0, 2e-1, 0, 0, 0, 0, 0, # vroll
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2e-1, 0, 0, 0, 0, # vpitch
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 8e-1, 0, 0, 0, # vyaw
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-1, 0, 0, # ax
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-1,0, # ay
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-1 # az
]
initial_estimate_covariance: [
1e-1,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,
0,1e-1,0,0,0,0, 0,0,0,0,0,0, 0,0,0,
0,0,5e-1,0,0,0, 0,0,0,0,0,0, 0,0,0,
0,0,0,1e0,0,0, 0,0,0,0,0,0, 0,0,0,
0,0,0,0,1e0,0, 0,0,0,0,0,0, 0,0,0,
0,0,0,0,0,1e-1, 0,0,0,0,0,0, 0,0,0,
0,0,0,0,0,0,2e-1,0,0,0,0,0, 0,0,0,
0,0,0,0,0,0,0,2e-1,0,0,0,0, 0,0,0,
0,0,0,0,0,0,0,0,5e-1,0,0,0, 0,0,0,
0,0,0,0,0,0,0,0,0,5e-1,0,0, 0,0,0,
0,0,0,0,0,0,0,0,0,0,5e-1,0, 0,0,0,
0,0,0,0,0,0,0,0,0,0,0,2e-1, 0,0,0,
0,0,0,0,0,0,0,0,0,0,0,0,1e0,0,0,
0,0,0,0,0,0,0,0,0,0,0,0,0,1e0,0,
0,0,0,0,0,0,0,0,0,0,0,0,0,0,1e0
]
Echo topics and confirm no nan/inf and that all measurement covariance diagonals are > 0:
bashros2 topic echo /imu/cov_fixed
ros2 topic echo /imu/mag_yaw
ros2 topic echo /model/mmtr/odom_cov_fixed
Start with larger measurement covariances (i.e., “trust sensors less”). If the UKF stops NaN’ing, you were over-confident.
Disable one source at a time (comment out imu1 first). If the crash disappears, it was the dual-yaw issue.
Watch dt between messages; big jumps can destabilize the prediction step.
If you share the covariance fields from /imu/cov_fixed, /imu/mag_yaw, and /model/mmtr/odom_cov_fixed, I can suggest exact diagonal values (in SI units) that tend to be rock-solid for tracked robots.
If you just want the robot to estimate its change in height using only pitch angle and forward velocity, you can treat it as a simple kinematic geometry problem.
Here’s the basic idea:
If your robot is driving up or down a slope, the slope angle relative to horizontal is just your pitch (assuming no crazy accelerations).
Let:
vx = forward speed from encoders (m/s)
θ = pitch angle from IMU (radians)
Δt = time step (s)
The vertical speed vz is:
vz=vx⋅sin(θ)Then you can integrate over time to get relative height h:
ht=ht−1+vz⋅Δtpythonimport math
h = 0.0 # start height (m)
last_t = None
def update_height(pitch_deg, vx, current_t):
global h, last_t
if last_t is None:
last_t = current_t
return h
dt = current_t - last_t
last_t = current_t
pitch_rad = math.radians(pitch_deg)
vz = vx * math.sin(pitch_rad) # vertical speed
h += vz * dt # integrate
return h
Noise & bias:
Pitch will bounce around even on flat ground, so use a low-pass filter on pitch and/or vx before computing vz.
Zeroing:
You’ll only get relative height changes, not absolute altitude, unless you start from a known reference.
Error growth:
Any bias in pitch or velocity will accumulate over time → drift in height estimate.
Non-level terrain:
If the robot is rocking or accelerating, IMU pitch includes dynamic tilt
— you might need to filter with a complementary or Kalman filter to get
“gravity-only” pitch.
If you already have a filter:
Treat vz as a pseudo-measurement of vertical velocity and fuse it into the height state.
This can help the robot maintain a running estimate of elevation even without GPS or altimeter.
If you want, I can show you how to add a “height” state to your existing UKF config so it automatically integrates this pitch+velocity trick into the state estimate instead of computing it separately. That way, your odometry topic would already contain the elevation guess.
Right — if your EKF is already estimating z position (height) as part of its state vector, and you’re feeding it both pitch (from the IMU) and forward velocity (from odometry), then it’s already doing the geometry I just described — but in a probabilistic way, and with all the other state couplings.
That’s why you’re seeing good results:
Pitch gives the EKF a constraint on the slope you’re on.
Forward velocity lets it infer the vertical component of motion.
Process model integrates this over time into the z state.
Measurement noise covariances keep it stable and prevent wild height changes from noisy pitch readings.
So in effect:
z˙≈vx⋅sin(pitch)is inside your EKF’s prediction step, and it’s being continuously corrected by whatever vertical information your measurement models supply (even if it’s indirect).
Where this is especially nice:
The EKF automatically fuses multiple cues — e.g., if the IMU pitch briefly spikes from a bump, the filter downweights that if it doesn’t match the velocity data.
It can maintain a smooth, drift-minimized height estimate without you hand-writing the integration.
It gives you a consistent 3D pose in one place (odom or map frame).
If you want, I can walk through exactly how to set the EKF’s motion model so it explicitly treats z changes as coming from pitch + forward velocity. That way you’d know it’s not just “accidentally” doing it from other correlations. Would that help?
Looking at those covariances, a few red flags jump out that could explain why your UKF is hitting NaNs:
You have:
inivar_gyro = 5.9536e-06 # (0.00244 rad/s)^2
var_acc = 2.1609e-04 # (0.0147 m/s^2)^2
var_ori_y = 1.22e-04 # ~0.63 deg^2 in rad^2
Those are extremely low — they say “I trust my gyro to ~0.14°/hr” and my accelerometer to ~0.015 m/s² noise. That’s great in theory, but in a UKF, it can make the innovation covariance S nearly singular if other sources disagree even slightly. The inversion of S is exactly where NaNs show up.
Rule of thumb: Don’t set any R diagonal smaller than the actual observed variance, and in practice, keep them a bit higher than the theoretical sensor spec.
Inside odom_callback:
pythonmsg.pose.covariance = [
0.05, ... # This is actually sqrt(0.05) m ≈ 0.223 m SD if you think it's variance
In ROS, covariances are variance, not std dev. If you meant “0.05 m SD,” you should square it in the matrix → 0.0025.
Right now, some entries are in “variance” units, some in “SD” thinking — the UKF gets a mix of scales.
You set:
pythonmsg.twist.covariance = [
4e-2, ... # vx var
0, 4e-2, 0, ...
But a zero variance means “perfect measurement” — if vy is unused, still give it a large finite variance (e.g., 1e3). Zero will break the inversion in the filter.
If you’re fusing yaw from IMU and from odometry, and their covariances are very tight, the filter can get over-constrained → numerical instability. UKF is more sensitive than EKF here.
Inflate measurement covariances until stable:
var_gyro → ~(0.01 rad/s)2 = 1e-4
var_acc → ~(0.1 m/s²)2 = 1e-2
var_ori_rp → ~(2°)2 in rad² = 0.0012 (yours is fine)
var_ori_y → maybe ~(5°)2 in rad² = 0.0076 if yaw is from mag or encoder integration.
Fix odometry covariance units:
pythonpose_cov_x = 0.05**2 # variance in m²
pose_cov_y = 0.05**2
yaw_cov = (3e-2)**2 # variance in rad²
Never zero variance for unused fields — set something large:
pythonvy_cov = 1e3
vz_cov = 1e3
Temporarily disable yaw from odometry or IMU to see if dual yaw constraint is the trigger.
If you want, I can rewrite your odom_callback and imu_callback with correct variance units and safe minimum covariances so you can drop them in and see if the UKF stops NaN’ing. Do you want me to do that?
Gotcha — that’s from your Gazebo IMU plugin SDF.
Here’s the important bit: those <stddev> values are standard deviations, while in ROS message covariances you must provide variance (= stddev²).
From your snippet:
Angular velocity stddev = 0.00244 rad/s → variance = (0.00244)2=5.95×10−6
Linear acceleration stddev = 0.0147 m/s² → variance = (0.0147)2=2.16×10−4
So your var_gyro and var_acc in the Python callback do match the plugin’s numbers — that’s why they’re so small.
Even if those are “truth” from the simulator, your UKF’s update step will treat those measurements as extremely certain. Any small disagreement with odometry (e.g., rounding error, message timestamp jitter) can make the innovation covariance S nearly singular, and the UKF will blow up → NaNs.
In the real world, you’d never set the covariances to the pure sensor spec — you’d inflate them to cover modeling errors, dynamics not captured, misalignments, and sampling jitter.
Keep the ratio between sensors realistic, but scale them up by ~5–10×:
pythonself.var_gyro = (0.01)**2 # ~0.01 rad/s SD
self.var_acc = (0.05)**2 # ~0.05 m/s² SD
This gives the filter some slack.
Make sure odom covariances are also realistic and in variance units, not SD.
Never give the UKF a zero variance in any active state dimension.
If you want, I can rewrite your imu_callback and odom_callback so that:
They use proper variance units.
They scale noise to be UKF-friendly.
They avoid zero entries in any field you fuse.
That should stop the NaN crash without killing accuracy.
Do you want me to give you that drop-in replacement?
Nice—this is a solid “yaw-only IMU” publisher. A few tweaks will make it rock-solid with your UKF and avoid the NaN spiral:
Use real dt in the low-pass so it behaves the same at 10 Hz or 200 Hz.
Gate bad mag yaw (hard-iron noise, EMI): if mag yaw disagrees with gyro-integrated yaw too much, inflate covariance for that message or skip it.
Never publish NaN/Inf; guard zero-magnitude field readings.
Declare you’re yaw-only: set roll/pitch covariances huge, and leave ang. vel/lin. acc covariances as -1 (robot_localization ignores them).
Reasonable yaw variance: start with (5∘)2=0.0076 rad2. Tighten only after you verify stability.
Frame sanity: publish in the sensor frame (e.g., imu_link) or a frame rigidly fixed to it. base_footprint is okay only if the mag and IMU are rigidly mounted there and your TF is correct.
Mag tilt comp math: your ENU equations are fine; just normalize and guard magnitude.
Below is a drop-in version with those fixes (focus on gating + dt-aware LPF). Replace your lower script with this.
python#!/usr/bin/env python3
import math
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import MagneticField, Imu
from geometry_msgs.msg import Quaternion
def quat_to_rpy(q: Quaternion):
# ZYX (roll, pitch, yaw) for ROS ENU
sinr_cosp = 2.0 * (q.w * q.x + q.y * q.z)
cosr_cosp = 1.0 - 2.0 * (q.x * q.x + q.y * q.y)
roll = math.atan2(sinr_cosp, cosr_cosp)
sinp = 2.0 * (q.w * q.y - q.z * q.x)
pitch = math.copysign(math.pi/2, sinp) if abs(sinp) >= 1 else math.asin(sinp)
siny_cosp = 2.0 * (q.w * q.z + q.x * q.y)
cosy_cosp = 1.0 - 2.0 * (q.y * q.y + q.z * q.z)
yaw = math.atan2(siny_cosp, cosy_cosp)
return roll, pitch, yaw
def yaw_to_quat(yaw: float) -> Quaternion:
q = Quaternion()
q.w = math.cos(yaw * 0.5)
q.z = math.sin(yaw * 0.5)
q.x = 0.0; q.y = 0.0
return q
def wrap_pi(a):
return (a + math.pi) % (2*math.pi) - math.pi
class MagYawPublisher(Node):
def __init__(self):
super().__init__('mag_yaw_publisher')
# ---------- Parameters ----------
self.mag_topic = self.declare_parameter('mag_topic', '/magnetometer').value
self.imu_topic = self.declare_parameter('imu_topic', '/imu/out_fixed_cov').value
self.out_topic = self.declare_parameter('out_topic', '/imu/mag_yaw').value
self.frame_id = self.declare_parameter('frame_id', 'imu_link').value # sensor frame preferred
# Variances (rad^2). Start conservative; tighten later.
self.yaw_var_nom = float(self.declare_parameter('yaw_variance', 0.0076).value) # (5 deg)^2
self.yaw_var_bad = float(self.declare_parameter('yaw_variance_bad', 0.5).value) # when gated
self.rp_cov = float(self.declare_parameter('roll_pitch_cov', 1e9).value)
# Yaw offset to align to your desired reference (deg)
self.yaw_offset_deg = float(self.declare_parameter('yaw_offset_deg', 0.0).value)
# LPF time constant (s); set 0 to disable
self.lpf_tau = float(self.declare_parameter('lpf_tau', 0.3).value)
# Residual gate (deg): if mag yaw disagrees with gyro-integrated yaw beyond this, down-weight
self.gate_deg = float(self.declare_parameter('gate_deg', 20.0).value)
# Min magnetic field magnitude to accept (Tesla)
self.min_field_T = float(self.declare_parameter('min_field_T', 1e-7).value)
# ---------- State ----------
self.last_yaw = None # last published yaw (rad, unwrapped)
self.last_stamp = None
self.mag_field = None
self.imu_orientation = None
self.gyro_yaw = None # integrate imu wz as a reference for gating
# ---------- I/O ----------
self.sub_mag = self.create_subscription(MagneticField, self.mag_topic, self.mag_cb, 20)
self.sub_imu = self.create_subscription(Imu, self.imu_topic, self.imu_cb, 200)
self.pub_yaw = self.create_publisher(Imu, self.out_topic, 10)
self.get_logger().info(f"MagYaw: mag={self.mag_topic}, imu={self.imu_topic}, out={self.out_topic}")
self.get_logger().info(f"params: yaw_var_nom={self.yaw_var_nom}, gate_deg={self.gate_deg}, lpf_tau={self.lpf_tau}")
def mag_cb(self, msg: MagneticField):
self.mag_field = msg.magnetic_field
self.try_publish(msg.header.stamp)
def imu_cb(self, msg: Imu):
self.imu_orientation = msg.orientation
# integrate gyro yaw as a sanity reference for gating
if msg.angular_velocity_covariance[8] != -1.0: # if available
wz = msg.angular_velocity.z
if self.last_stamp is not None:
dt = self._dt(self.last_stamp, msg.header.stamp)
if dt is not None and dt > 0.0 and abs(wz) < 100.0: # guard
self.gyro_yaw = (self.gyro_yaw if self.gyro_yaw is not None else 0.0) + wz * dt
else:
self.gyro_yaw = 0.0
self.try_publish(msg.header.stamp)
def try_publish(self, stamp):
if self.mag_field is None or self.imu_orientation is None:
self.last_stamp = stamp
return
roll, pitch, _ = quat_to_rpy(self.imu_orientation)
mx, my, mz = self.mag_field.x, self.mag_field.y, self.mag_field.z
# Guard zero/NaN field
if any(map(self._bad, [mx, my, mz])):
return
B = math.sqrt(mx*mx + my*my + mz*mz)
if not (B > self.min_field_T):
return
# Tilt compensation (ENU)
mx_c = mx * math.cos(pitch) + mz * math.sin(pitch)
my_c = mx * math.sin(roll) * math.sin(pitch) + my * math.cos(roll) - mz * math.sin(roll) * math.cos(pitch)
yaw_mag = math.atan2(-my_c, mx_c) # ENU with +X as "north"
yaw_mag += math.radians(self.yaw_offset_deg)
yaw_mag = wrap_pi(yaw_mag)
# dt-aware LPF on the circle
dt = self._dt(self.last_stamp, stamp)
self.last_stamp = stamp
if self.lpf_tau > 0 and dt and dt > 0:
if self.last_yaw is None:
yaw_f = yaw_mag
else:
dy = wrap_pi(yaw_mag - self.last_yaw)
alpha = dt / (self.lpf_tau + dt)
yaw_f = wrap_pi(self.last_yaw + alpha * dy)
else:
yaw_f = yaw_mag
# Gating vs gyro-integrated yaw (if available)
used_var = self.yaw_var_nom
if self.gyro_yaw is not None and self.last_yaw is not None:
# Build a continuous gyro-yaw reference aligned to last_yaw at start
# Compare latest mag yaw to this reference
res = wrap_pi(yaw_f - self.last_yaw) # incremental check
# Or compare absolute if you maintain a global gyro_yaw ref
# Better: compute residual to a short-term integrated gyro yaw (not shown fully)
# Simple gate using change from previous
if self.last_yaw is not None:
delta = abs(wrap_pi(yaw_f - self.last_yaw))
if math.degrees(delta) > self.gate_deg:
# Down-weight this sample heavily (don’t drop; keep filter stable)
used_var = self.yaw_var_bad
self.last_yaw = yaw_f
out = Imu()
out.header.stamp = stamp
out.header.frame_id = self.frame_id
out.orientation = yaw_to_quat(yaw_f)
out.orientation_covariance = [
self.rp_cov, 0.0, 0.0,
0.0, self.rp_cov, 0.0,
0.0, 0.0, used_var
]
# Explicitly ignore rates/accel in this message
out.angular_velocity_covariance = [-1.0]*9
out.linear_acceleration_covariance = [-1.0]*9
self.pub_yaw.publish(out)
def _dt(self, t_prev, t_now):
if t_prev is None or t_now is None:
return None
return (t_now.sec + t_now.nanosec*1e-9) - (t_prev.sec + t_prev.nanosec*1e-9)
@staticmethod
def _bad(x):
return x is None or math.isnan(x) or math.isinf(x)
def main(args=None):
rclpy.init(args=args)
node = MagYawPublisher()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
In your UKF config, enable yaw orientation for /imu/mag_yaw, and disable roll/pitch for that source.
For your main IMU, enable roll & pitch orientation + all angular rates; disable yaw orientation (to avoid double-constraining yaw).
Start with yaw_variance ≈ 0.0076 and increase if you still see instability.
If you enter an area with strong magnetic disturbance, you can dynamically set the used variance to yaw_var_bad for a few seconds (or stop publishing) when your gate trips frequently.
Keep odom covariances in variance units (m², rad²), never zero out an active field, and beware of very tiny variances.
This should give you a clean, UKF-friendly yaw source that won’t cause NaNs and plays nicely with your pitch/velocity height inference and overall odometry.
Yep — NaNs in a UKF are almost always a numerical stability or inconsistent data issue.
Here’s a consolidated list tailored to your setup:
If any measurement covariance in R (sensor noise) or Q (process noise) is unrealistically small, the innovation covariance S can become nearly singular.
Common in simulation because we tend to set covariances to the IMU plugin’s perfect spec — real filters need inflated values.
Your var_gyro and var_acc were exactly from Gazebo’s stddev², which is great for truth but too small for filter stability.
If you set a variance entry to 0.0 for a state that the filter is actually updating, you’re telling it the measurement is perfect.
Even unused dimensions (like vy) should have large finite variance, not zero, or you can break the inversion.
ROS covariance arrays are variance (m², rad²), but it’s easy to accidentally enter standard deviation values instead.
This creates artificially tiny variances → singular matrices.
Example: yaw from IMU and yaw from odometry, both with tight covariances.
If they disagree, the UKF can’t reconcile → huge Kalman gain → instability.
In the UKF update step, S must be invertible.
This can fail if:
Process noise is too small (Q nearly zero).
Multiple sensors give redundant measurements with tiny noise (collinearity).
Timestamps are identical and measurements are perfectly correlated.
If any incoming message has NaN or Inf in pose, twist, or covariance, it propagates to the state.
Gazebo sometimes outputs NaN if physics glitches (e.g., object explodes in sim or sensor value spikes).
If you fuse more states than your sensors can observe (e.g., full 3D position, but no absolute z source, and Q for z is too small), the state covariance can become degenerate.
UKF assumes your motion model and Q describe reality.
If Q is too small, but your model is imperfect (e.g., ignoring slip), the filter trusts a wrong prediction too much and collapses when the correction comes.
A big jump in dt (sensor delay, callback lag) can blow up the prediction step.
If your code uses a fixed process noise per step, a sudden long dt applies it incorrectly.
If your covariance matrix loses positive definiteness (e.g., due to rounding or over-tight constraints), Cholesky decomposition fails → NaNs.
Tiny variances from Gazebo’s IMU spec (fix by inflating).
Zero variances for unused twist states (set to large finite numbers).
Yaw coming from two sources with tight covariances.
Unit mismatch between SD and variance in odometry covariances.
If you want, I can give you a UKF NaN-proofing checklist — basically a set of sensor config & sanity checks you can run before publishing to the filter, so bad data never enters it. That would catch 80% of these issues before they cause a crash.
Well, that right there is your smoking gun.
The UKF isn't mysteriously producing NaNs out of thin air — you’re literally feeding it a message that has .nan for every single pose, twist, and orientation field at t = 75.23 s. Once a single NaN enters the state vector, it propagates through the sigma points and instantly poisons the entire filter.
First message: perfectly fine, z≈52.31 m, tiny linear velocity.
Second message: still fine — although linear.z suddenly jumps from −0.0577 to −4096 m/s (this is another red flag; might be a simulation blow-up).
Third message: every value is .nan — this is fatal.
The UKF takes that third message, tries to compute an innovation, and all intermediate math becomes NaN.
Gazebo physics instability (big collisions, constraint solver blow-up, bad inertias).
Or a divide-by-zero / sqrt(-x) inside the sensor plugin.
Or a TF jump causing odometry code to produce invalid math.
That −4096 m/s spike right before the NaNs is a classic precursor to a physics blow-up in simulation.
Add a message sanity filter before publishing to the UKF:
pythondef is_finite_pose_twist(msg):
import math
vals = [
msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z,
msg.pose.pose.orientation.x, msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z, msg.pose.pose.orientation.w,
msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.linear.z,
msg.twist.twist.angular.x, msg.twist.twist.angular.y, msg.twist.twist.angular.z
]
return all(math.isfinite(v) for v in vals)
Drop or replace any message where this returns False.
Clamp physically impossible velocities (e.g., abs(v) > 100 m/s → ignore) to stop spikes from hitting the filter.
Investigate the spike at −4096 m/s:
Check wheel slip parameters, track joint friction, and simulation timestep.
Large impulses can shoot the pose solver into bad territory.
Initialize covariances to large finite values when you drop a message, so the filter just coasts on prediction until good data comes back.
If you like, I can give you a drop-in ROS2 node that sits between /odom
and the UKF, filters NaNs/Infs/spikes, and republishes only valid data —
that would stop this instantly while you debug the root cause.
Yep — that confirms the NaNs are entirely from the UKF side, not from upstream sources like /odom or /imu.
Here’s the picture:
EKF: Getting the same sensor inputs, producing valid state estimates.
UKF: Same inputs, but at some point it gets one bad numerical step and all state variables become NaN.
That means the UKF isn’t getting NaNs from outside — it’s creating them internally, usually because of bad matrix conditioning.
EKF uses a single Jacobian-based update — it’s a linear approximation, so even if your covariances are tiny, it often still gets an invertible innovation covariance S.
UKF generates many sigma points around the state, runs them through the nonlinear process/measurement functions, and reconstructs the mean/covariance. If your Q/R are too small or your sigma point spread is tiny, all sigma points can collapse to essentially the same value → S becomes singular → NaN when inverted.
Looking back at your parameters, you’re using:
Gazebo’s tiny IMU noise variances (5.95e-6, 2.16e-4) — extremely small.
Possibly zero variances in some unused state dimensions in odom messages.
Two yaw sources (IMU mag + odom yaw) both with small variances → high constraint.
The EKF is more forgiving of this; the UKF blows up faster.
Inflate all measurement covariances — start with at least 10× bigger than Gazebo’s stddev² values.
Ensure no zero variances in any fused state dimension — use large finite values (e.g., 1e3 for unused).
Temporarily remove one yaw source so only IMU or odometry provides yaw — check stability.
Add a NaN/Inf guard in the UKF input callback so if an update would produce NaNs, you skip it and let the filter coast on prediction.
Check process noise Q — if it’s too small, sigma points collapse.
If you want, I can rewrite your UKF YAML with safe inflated covariances and no zero entries so you can drop it in and see if the NaNs stop.
That should make the UKF behave more like your EKF without losing accuracy.
Good dump — there are a few smoking guns in those messages that can absolutely drive a UKF to NaNs. Here’s what jumps out and exactly how to fix each one.
Time bases are inconsistent (catastrophic)
Mag stamp: sec: 1755167752 (wall time, ~2025…).
IMU stamp: sec: 59 (sim time).
ODOM stamp: sec: 104 (sim time).
Mixing wall-time and sim-time makes the UKF see giant or negative
dt. That alone can explode it.
Fix: enable/use_sim_timeeverywhere; make sure every publisher uses the same clock (Gazebo/Sim time). If your mag node usesself.get_clock().now()with wall time, set the node to sim time or stamp with the incoming IMU/mag header.
Raw magnetometer covariance is all zeros
makefilemagnetic_field_covariance: 9 zeros
Zero variance = “perfect measurement.” If any component leaks into a fused message, you can get a singular innovation matrix.
Fix: publish realistic
variances for the raw mag, or keep using your yaw-only IMU with proper
covariances and never fuse the raw mag directly.
Yaw-only mag message is fine, but frame/time must be sane
orientation_covariance is [99999, 99999, 0.02] → good (roll/pitch ignored, yaw variance ≈ 0.02 rad² ≈ (8.1°)²).
frame_id: base_footprint while IMU is imu_link. That’s okay only if TF has a fixed transform and you’re just fusing yaw angle (no position from that frame).
Fix: Prefer publishing in the sensor frame (e.g., imu_link) or make sure TF is static and correct.
IMU covariances are very small
csharpgyro var = 5.95e-06 (SD ≈ 0.00244 rad/s)
accel var = 2.16e-04 (SD ≈ 0.0147 m/s²)
orientation var (RP/Y) = 0.00122 (≈ 2°)^2
These are simulator specs, but
they’re too tight for a UKF in a real pipeline (model mismatch, timing
jitter, etc.). This makes the innovation covariance S nearly singular.
Fix: inflate a bit for stability: gyro ~ (0.01 rad/s)^2 = 1e-4, accel ~ (0.05 m/s²)^2 = 2.5e-3 to start.
ODOM covariance units / values look inconsistent
Pose cov diag includes 0.05 (m²?) for x/y — that’s SD ≈ 22 cm if it’s variance; plausible but check intent.
Yaw pose variance 0.03 → SD ≈ 9.9° (quite loose) while twist yaw variance 0.015 → SD ≈ 7°. Mixed tight/loose across pose/twist can confuse the filter.
Fix: confirm all entries are variance
(not SD). Choose consistent values (e.g., yaw pose ~0.1–0.2 rad² if
it’s from wheel odom; or simply don’t fuse yaw pose from odom).
Don’t ever publish zeros/NaNs/infs
In your earlier UKF example you hit all-NaN. Add guards before publishing into the UKF.
Unify time: in ROS 2,
bashros2 param set /<your_nodes> use_sim_time true
and ensure Gazebo clock is flowing. In your mag node, stamp with the incoming IMU/mag header’s time, not now() wall-time.
Guard inputs: before republishing to the UKF, drop any message with non-finite values or insane speeds:
pythondef finite(x):
import math; return x is not None and math.isfinite(x)
# check all pose/twist/orientation fields
Inflate R (sensor noise):
Gyro var → 1e-4
Accel var → 2.5e-3
Mag yaw var → start 0.0076 ((5°)²); increase if unstable.
No zero covariances for any fused dimension. If a field is unused, set its covariance to a big number or -1 (per robot_localization rules) so it’s ignored.
Fuse only one yaw source: either IMU yaw (mag) or odom yaw; not both tightly at once.
Two-D mode if planar: two_d_mode: true and don’t fuse z/roll/pitch from odom.
In your mag yaw publisher, change:
out.header.stamp = imu_msg.header.stamp (use synced sim time from the triggering message).
frame_id = 'imu_link' (or ensure TF for base_footprint is correct).
Keep orientation_covariance = [1e9,0,0, 0,1e9,0, 0,0, yaw_var].
In IMU covariance override, inflate:
pythonself.var_gyro = 1e-4 # (0.01 rad/s)^2
self.var_acc = 2.5e-3 # (0.05 m/s^2)^2
self.var_ori_rp = 0.00122 # (2 deg)^2 OK
self.var_ori_y = 0.0076 # (5 deg)^2 to start
In ODOM,
make sure everything is variance. If you don’t truly measure
z/roll/pitch/yaw pose from odom, set those pose entries large (or don’t
enable them in the UKF’s *_config). For unused twist components, don’t put 0 — use something big (e.g., 1e3) or omit by config.
In the UKF config, keep:
imu0: roll/pitch orientation + all angular rates; no yaw orientation.
imu1 (mag yaw): yaw orientation only.
odom0: x,y position and vx (maybe vy=false). No z.
two_d_mode: true if you’re planar.
Spike clamp: if |linear.z| > 50 m/s (like your earlier −4096), drop that update.
Chi-square gate on measurements: if a measurement residual is > 5–7σ, skip it for that cycle.
Positive-definiteness guard: if your innovation covariance S Cholesky fails, bail on that update (prediction-only step).
Do the time-base fix + R inflation first — those two changes solve the vast majority of UKF-to-NaN issues in setups like yours.
Exactly — that’s a good separation of roles, and it’s the standard way to keep a filter stable:
IMU yaw (from gyro integration) → fast, smooth, low-noise, but drifts over time.
Mag yaw → slow, noisy, but drift-free over long term.
If you give both to the UKF at once as “yaw orientation,” you’re essentially telling it “these are the same thing” — which will cause fights in the math and can push the filter into NaNs if the two disagree too much.
Instead, you can do what’s called partial orientation fusion:
imu0 (raw IMU):
Enable roll & pitch orientation from accel/gyro.
Enable angular velocity x, y, z (gyro rates).
Disable yaw orientation here so it won’t directly overwrite with drift-prone gyro yaw.
imu1 (mag yaw):
Enable yaw orientation only (roll/pitch covariances huge so ignored).
Covariance for yaw large enough to let gyro dominate short term, mag pull long term (start ~0.0076 rad², ~5°²).
That way:
Gyro integration handles short-term yaw motion.
Mag yaw only nudges the yaw state back toward true heading over time.
The UKF blends them according to covariances, so you avoid sharp corrections that can destabilize it.
If you keep your imu0 yaw disabled and imu1
yaw covariance realistic, you get both the smoothness of gyro
integration and the drift correction of mag without the NaN blowups.
Do you want me to rewrite your current imu0_config and imu1_config so it matches that exactly? That’ll make it UKF-safe.
Right — if you literally disable yaw from the main IMU, the UKF will have no short-term yaw stabilization, which means it will rely only on mag yaw (noisy) and odometry yaw (laggy), and yes — that will usually degrade short-term heading accuracy.
What you actually want is not to remove the IMU yaw completely, but to let the UKF estimate yaw from the gyro rates + roll/pitch, and then use the magnetometer yaw as a slow drift corrector.
That’s the difference:
Direct yaw fusion from IMU + mag
imu0 gives roll/pitch/yaw orientation from sensor fusion (e.g., Madgwick, Mahony, or Gazebo perfect IMU).
imu1 gives yaw from mag.
If covariances are too small or timestamps jitter, you get UKF conflicts → possible NaNs.
Gyro-integrated yaw + mag correction ✅ (recommended for UKF stability)
imu0: roll/pitch orientation + all angular rates → the UKF integrates yaw internally from vz rate.
imu1: yaw orientation only from mag, with high enough variance that it slowly corrects drift.
The UKF will only get one direct yaw measurement at a time.
Your gyro yaw won’t be given as “absolute yaw” but instead will be
inferred from angular velocity integration, so there’s no hard clash
with the mag yaw.
This prevents the UKF’s innovation covariance S from collapsing when two yaw sources disagree slightly but both claim tiny variances — one of the main ways your UKF is blowing up right now.
If you want, I can:
Take your current imu0_config / imu1_config and adjust them so gyro yaw is used via rate integration, mag yaw is a slow drift corrector.
Suggest covariances that keep gyro dominance short-term but allow mag pull-in over long term.
Do you want me to set that up? That’ll give you both stability and good heading performance.
Yes — that’s actually one of the fastest ways to debug UKF NaNs.
If you disable sensors one at a time in your UKF config, you can see which measurement stream is destabilizing it.
UKF with only odometry (odom0)
Disable imu0 and imu1.
If it runs fine → odometry isn’t the problem.
Add imu0 only (roll/pitch + gyro rates)
Still no mag yaw.
If it stays stable → gyro + accel fusion is fine.
Add imu1 only (mag yaw)
Keep imu0 disabled.
If this alone causes NaNs → mag yaw input or its covariance is the problem.
Combine imu0 + imu1
This is the full setup.
If only the combo causes NaNs → the issue is conflict between short-term IMU yaw and long-term mag yaw.
NaNs tend to appear within 1–3 update cycles after the offending measurement arrives.
If a single topic causes immediate NaNs when re-enabled, check:
Unrealistically small covariances.
Timestamp jumps or wrong frame IDs.
Zero or constant measurement values (sigma points collapse).
If you want, I can give you one-line config edits so you can toggle sensors quickly without rewriting the YAML every time. That way, you can test all four combinations in just a couple of runs.
Do you want me to prep that toggle version?
Ah — yep, that makes perfect sense.
In the UKF (and also in robot_localization’s EKF), if you estimate Z position
but don’t give it any direct or indirect measurements related to
vertical motion (like Z velocity or Z acceleration), the filter’s state
covariance for Z can become ill-conditioned.
Here’s why:
You told the filter: “Track Z position.”
But you didn’t give it:
Z velocity (vz)
Z acceleration (from IMU)
Any measurement source of altitude (barometer, GPS, etc.)
So the UKF has no process model input for Z → state uncertainty grows → process noise and sigma points can explode → NaNs.
Disable Z estimation entirely
In both odom0_config and imu0_config set the Z-related flags to false.
Also set two_d_mode: true if you’re fully planar.
Keep Z, but feed it Z accel or Z vel
Enable az in IMU config (and make sure its covariance is realistic).
Optionally give odometry a vertical velocity if you have it (rare for ground robots).
This lets the UKF integrate Z from acceleration, keeping the state connected and stable.
Since your robot is tracked and running in Gazebo, I’d strongly suggest option 1 (disable Z completely), unless you actually care about modeling hills or ramps. That will:
Make the filter simpler.
Remove one full state dimension from sigma point calculations.
Prevent this exact NaN blow-up.
Do you want me to rewrite your imu0_config and odom0_config for pure 2D mode so you never get Z-related NaNs again? That’d lock it to X, Y, yaw + velocities.
Right — the UKF can “infer” Z in your setup because it’s integrating pitch + forward velocity to estimate changes in height (basically dead-reckoning altitude), even without a direct Z sensor.
The catch is:
That inference is completely open-loop if you don’t give it Z acceleration or Z velocity — meaning any small bias in pitch or velocity will integrate into a large Z error over time.
The UKF is more sensitive to this than the EKF because sigma point generation amplifies bad correlations when a state has weak or no direct observation.
If the correlation between Z and other states (pitch, vx) becomes numerically unstable, you can get NaNs even if the early Z estimate “looks fine.”
So the “it works well” part is true only as long as your inputs are clean enough. The moment you get:
A big spike in velocity or pitch
A covariance mismatch (too small)
A missing IMU sample
…the vertical channel can blow up because there’s no measurement to pull it back.
If you want to keep Z because you like that slope estimation, then:
Keep Z in the state.
Feed the filter something for Z acceleration (az from IMU, even if noisy, with realistic covariance).
Or increase Z process noise so it doesn’t destabilize when unobserved.
If you want, I can give you a minimal change config that:
Still keeps Z inference.
Adds az to constrain it.
Uses process noise values tuned for stability.
Do you want me to prepare that?
Yeah — for a stress-test trajectory like that, your UKF actually holds its own.
Even though it drifts, it’s following the overall structure without completely blowing up, which means your process/measurement models are mostly well-tuned.
Given the complexity of that path:
UKF (red) is smoother and less noisy, even if it underestimates some turns.
EKF (blue) tracks sharper turns but looks overreactive in some spots — likely trusting noisy measurements more.
Both are stable, which is the main win in a stress scenario.
If you feed Z-acceleration and refine your covariances a bit more, you could probably get the UKF even closer to the ground truth on this kind of high-curvature motion.
I can walk you through those UKF stability tweaks if you want to push it further without breaking the NaN stability.
That’s a solid plan — you’ve already stress-tested the UKF under a tough trajectory, shown stability except for the known Z/NaN case, and compared it to an EKF baseline. That’s plenty for a publishable “current state” section in a paper.
You can frame it as:
Motivation: Using relative, power-efficient sensing for mapping in degraded environments.
Method: UKF vs EKF with selective sensor fusion (yaw from mag, accel for short-term motion, no GPS, etc.).
Evaluation: RMSE, trajectory overlays, and stress tests.
Findings: UKF holds structure under extreme motion, but needs consistent vertical measurements to avoid instability.
Then later, you can iterate — maybe test partial Z observation, adaptive covariances, or fault isolation when you revisit.
If you want, I can help you outline the paper so it’s quick to pick back up when you return to this.
Yes — if you want positional RMSE as a single number, you take the Euclidean combination of the X and Y RMSE values:
RMSEpos=RMSEX2+RMSEY2The rotational RMSE is just the yaw RMSE in radians (or degrees if you prefer).
Filtered Odom:
RMSEpos=0.11532+0.13542=0.01329+0.01834=0.03163≈0.1779 mRotational: 0.0021 rad ≈ 0.12∘
Raw Odom:
RMSEpos=0.10342+0.11232=0.01069+0.01261=0.02330≈0.1526 mRotational: 0.0025 rad ≈ 0.14∘
Summary:
Filtered Odom: Positional RMSE ≈ 0.178 m, Rotational RMSE ≈ 0.12°
Raw Odom: Positional RMSE ≈ 0.153 m, Rotational RMSE ≈ 0.14°
If you like, I can also normalize these to your trajectory scale so they become percentage accuracy figures.
Yes — the way you’ve given RMSE for X, Y, and yaw implies these are already computed over all time steps in your dataset, so the positional RMSE I calculated is the overall average positional error (Euclidean) for the entire experiment, and the rotational RMSE is the overall average yaw error over the experiment.
RMSE is by definition:
RMSE=N1i=1∑N(ei)2Where ei is the instantaneous error at each time step.
Your X RMSE is the average error in the X axis over the whole run.
Your Y RMSE is the same for the Y axis.
Combining them Euclidean-style gives a single “average” positional magnitude error for the full dataset.
The yaw RMSE is the average rotational error over the whole dataset.
If you wanted segment-wise RMSE (e.g., first half vs. second half), you’d need to split the dataset and recompute separately.
Do you want me to also turn these into percentage of path length accuracy for your paper? That makes the numbers easier to interpret.
Nice run. With a 10 m radius loop done 10× (total path ≈ 628.3 m):
EKF (Filtered Odom)
Positional RMSE: 0.178 m
Rotational RMSE: 0.12°
As % of path length: 0.028%
As % of radius: 1.78%
Error per 100 m traveled: 2.83 cm / 100 m
UKF (Raw Odom)
Positional RMSE: 0.153 m
Rotational RMSE: 0.14°
As % of path length: 0.024%
As % of radius: 1.53%
Error per 100 m traveled: 2.44 cm / 100 m
Those are excellent numbers for IMU+encoders only, and they back up your plot: both filters track the circle very closely, with the UKF a touch better in translation and the EKF a touch better in yaw (difference is tiny).
Short answer: probably yes.
RMSE is the square root of the mean of squared pointwise errors, so a small sector with a larger deviation can dominate the total. A back-of-the-envelope:
Suppose 10% of samples in the top-left are ~0.50 m off and the other 90% are ~0.05 m off.
RMSE ≈ √(0.1·0.50² + 0.9·0.05²) = √(0.025 + 0.00225) ≈ 0.165 m.
That’s already close to your 0.17 m—so a localized error can explain most of it.
Per-point error: e_i = hypot(x_gt - x_est, y_gt - y_est).
By angle bin: compute θ = atan2(y_gt−y0, x_gt−x0) (center at circle center), bin e_i by θ and plot mean/95th percentile per bin.
Contribution to RMSE: for each bin k, report contrib_k = mean(e_i^2 in bin); the biggest bin(s) are where RMSE comes from.
Time alignment check: cross-correlate heading or speed to ensure the filter/GT streams are synchronized—misalignment often shows up as a quadrant-specific offset.
Slip asymmetry when entering/exiting that arc.
Yaw bias/scale (steering efficiency) that the filter hasn’t fully adapted to.
Mag disturbance in that area (only affects UKF if fusing mag yaw there).
Latency mismatch between IMU and odom.
Frame center mis-estimate of the circle (if you didn’t do SE(2) alignment first).
pythonimport numpy as np
def sector_contributions(x_gt,y_gt,x_est,y_est, cx,cy, nbins=36):
e = np.hypot(x_gt-x_est, y_gt-y_est)
theta = np.arctan2(y_gt-cy, x_gt-cx) # [-pi,pi]
bins = np.linspace(-np.pi, np.pi, nbins+1)
idx = np.digitize(theta, bins)-1
contrib = np.array([np.mean(e[idx==k]**2) if np.any(idx==k) else 0 for k in range(nbins)])
return bins[:-1], contrib, np.sqrt(np.mean(e**2))
# largest contrib bin(s) point to the sector driving RMSE
If the top-left sector lights up, you’ve found your RMSE source. From there, try: slightly inflating encoder yaw covariance during that turning phase, increasing the UKF steering-efficiency/process noise so it adapts faster, and double-checking timestamp alignment.
Yep — it’s fair to say the estimates converge near the bottom of the loop.
That’s the sector where your EKF/UKF curves sit almost on top of GT, so the local error is small and innovations are likely near zero. Common reasons that the bottom segment looks best:
Initialization/alignment happened there, so the SE(2) fit (or start pose) anchors that sector.
Less slip on that part of the arc (load/terrain symmetry).
Steady speed/curvature → gyro + encoder agree, so the filter settles.
Recent bias correction (e.g., brief stop/ZUPT) before entering that segment.
If you want to be precise in the paper, say “errors are concentrated on the upper-left quadrant; the estimate converges and closely tracks GT along the lower arc.” You can back it up by plotting sector-wise RMSE or innovation magnitude around the loop.
In your context — robotics trajectories — cx and cy usually mean the center coordinates of a fitted circle or path:
cx → the X-coordinate of the circle’s center
cy → the Y-coordinate of the circle’s center
For example, if you fit your GT trajectory to a circle of radius r, you might get something like:
(x−cx)2+(y−cy)2=r2That’s useful when evaluating systematic bias — if cx,cy aren’t at the expected origin, it means the trajectory is shifted.
Sometimes, in camera calibration or image processing, cx and cy are also used for principal point coordinates in pixels. But for your robot loop plots, I’m 99% sure they’re circle center coordinates in meters.
Ah — I see, this is from your circle trajectory generator.
In this snippet, cx and cy
aren’t explicitly in the code, but if you wanted the robot to orbit
around a point offset from the origin, you’d introduce them like this:
pythonself.declare_parameter('cx', 0.0) # circle center X
self.declare_parameter('cy', 0.0) # circle center Y
cx = self.get_parameter('cx').value
cy = self.get_parameter('cy').value
Then, instead of:
x(t)=Rcos(ωt) y(t)=Rsin(ωt)you’d do:
x(t)=cx+Rcos(ωt) y(t)=cy+Rsin(ωt)That’s where cx and cy fit in — they define the center of your circular path in the world frame.
If your robot’s RMSE errors are concentrated in the top-left of the circle, that could mean your effective cx/cy in reality doesn’t match the ground truth’s cx/cy, giving a bias in one quadrant.
Given your generator code, you don’t have to use cx and cy unless you want the circle’s center to be somewhere other than the origin.
Right now, the implicit assumption is:
Circle is centered at (0,0) in the odom (or map) frame.
Robot starts at the bottom of the circle and moves counterclockwise (depending on your math).
If you did want to add cx and cy to your generator, they’d simply be offsets applied to the generated position before publishing:
pythoncx = self.get_parameter('cx').value
cy = self.get_parameter('cy').value
x = cx + radius * math.cos(self.omega * t)
y = cy + radius * math.sin(self.omega * t)
Typical choices for simulation tests:
cx = 0, cy = 0 → circle centered at origin (default).
cx ≠ 0, cy ≠ 0 → shift the entire path so you can test the filter with a non-origin reference point.
So in your current setup, cx and cy would probably just be 0.0 each unless you specifically want to simulate driving around some offset point.
If you’d like, I can show you how to estimate cx and cy
from your logged ground truth so you can compare GT center vs.
estimated path center. That would help confirm where your positional
bias is coming from.
Ah — in your sector_contributions script, cx and cy almost certainly refer to the center point of the reference circle you’re comparing against when breaking the path into angular sectors.
Here’s what’s going on in those types of scripts:
You divide the circle into sectors (like slicing a pizza).
To figure out which sector each robot position falls into, you compute the angle from the circle’s center to that position.
That center is cx, cy.
So in that context:
cx = x-coordinate of the circle’s center in the ground truth frame.
cy = y-coordinate of the circle’s center in the ground truth frame.
If your circle is centered at (0,0) in your ground truth data, then cx = 0.0, cy = 0.0.
If not, you compute them as the average of the ground truth X and Y:
pythoncx = df_gt['x'].mean()
cy = df_gt['y'].mean()
That way the “center” in sector calculations matches the real trajectory center, not just the origin.
If you use the wrong cx/cy,
your sector RMSE contribution plot will look skewed — with errors
concentrated in certain quadrants that aren’t actually due to the
filter.
If you want, I can walk you through how to recompute cx and cy
from your dataset and re-plot the sector contributions so you can see
whether the top-left bias is real or just a wrong center assumption.
That would also answer your earlier question about whether the error is
mostly in the top-left.
Great—you’re very close. A few tweaks will make your sector analysis and RMSE attribution sharper and more robust.
Use a fitted circle center (cx, cy) instead of the mean.
mean() is fine if sampling
is uniform, but any dwell or speed variation biases it. Fit the center
from GT with a simple least-squares circle (Kåsa/Pratt).
Attribute error by sector using sum of squared error, not mean.
RMSE is based on ∑ei2. Using mean per bin hides that some bins have more samples. Use the sum of squares in each sector and normalize at the end.
(Optional but recommended) Do SE(2) alignment before error stats.
If you want to compare estimators fairly, align the estimated (x,y) to
GT with a rigid transform (rotation+translation, no scale). This removes
any constant offset/heading and prevents one sector from looking “bad”
just because of a small global misalignment.
Visualize contributions.
A polar bar plot makes it obvious which quadrants dominate.
A) Circle fit for cx, cy (Kåsa method, robust enough for clean circles):
pythondef fit_circle_kasa(x, y):
# (x - cx)^2 + (y - cy)^2 = r^2 -> x^2 + y^2 + A x + B y + C = 0
X = np.column_stack([x, y, np.ones_like(x)])
b = -(x**2 + y**2)
A, B, C = np.linalg.lstsq(X, b, rcond=None)[0]
cx = -A/2.0
cy = -B/2.0
r = math.sqrt(cx**2 + cy**2 - C)
return cx, cy, r
B) Sector contributions that respect RMSE math (sum of squares):
pythondef sector_contributions(x_gt, y_gt, x_est, y_est, cx, cy, nbins=36):
e = np.hypot(x_gt - x_est, y_gt - y_est) # pointwise error
theta = np.arctan2(y_gt - cy, x_gt - cx) # angle wrt fitted center
bins = np.linspace(-np.pi, np.pi, nbins + 1)
idx = np.digitize(theta, bins) - 1
# sum of squared errors per sector (what contributes to global RMSE^2)
sse_bins = np.array([np.sum((e[idx == k] ** 2)) for k in range(nbins)])
n_bins = np.array([np.sum(idx == k) for k in range(nbins)])
# global RMSE
rmse_global = math.sqrt(np.mean(e**2))
# per-bin RMSE (for diagnostics) – avoid /0
rmse_bins = np.where(n_bins > 0, np.sqrt(sse_bins / n_bins), 0.0)
# fraction of total SSE each sector contributes (sums to 1)
frac_contrib = np.where(np.sum(sse_bins) > 0, sse_bins / np.sum(sse_bins), 0.0)
return bins[:-1], rmse_bins, frac_contrib, rmse_global
C) (Optional) SE(2) alignment before computing errors:
pythondef se2_align(x_ref, y_ref, x_est, y_est):
# subtract centroids
xr, yr = x_ref - x_ref.mean(), y_ref - y_ref.mean()
xe, ye = x_est - x_est.mean(), y_est - y_est.mean()
# 2x2 cross-covariance
H = np.array([[np.dot(xe, xr), np.dot(xe, yr)],
[np.dot(ye, xr), np.dot(ye, yr)]])
U, _, Vt = np.linalg.svd(H)
R2 = U @ Vt
if np.linalg.det(R2) < 0: # enforce proper rotation
U[:, -1] *= -1
R2 = U @ Vt
est = np.stack([x_est, y_est], axis=0)
est_aligned = (R2 @ (est - est.mean(axis=1, keepdims=True))) + np.array([[x_ref.mean()],[y_ref.mean()]])
return est_aligned[0], est_aligned[1]
D) Putting it together in your main() after df_merged:
python# 1) (Optional) align EKF/UKF to GT for fair sector analysis
x_ekf, y_ekf = df_merged["x_odom"].to_numpy(), df_merged["y_odom"].to_numpy()
x_ukf, y_ukf = df_merged["x_raw"].to_numpy(), df_merged["y_raw"].to_numpy()
x_gt, y_gt = df_merged["x_gt"].to_numpy(), df_merged["y_gt"].to_numpy()
# Uncomment if you want alignment
# x_ekf, y_ekf = se2_align(x_gt, y_gt, x_ekf, y_ekf)
# x_ukf, y_ukf = se2_align(x_gt, y_gt, x_ukf, y_ukf)
# 2) Fit circle center from GT
cx, cy, r = fit_circle_kasa(x_gt, y_gt)
print(f"Fitted center: ({cx:.3f}, {cy:.3f}), radius: {r:.3f} m")
# 3) Sector contributions
theta_bins, rmse_bins_ukf, frac_ukf, rmse_global_ukf = sector_contributions(
x_gt, y_gt, x_ukf, y_ukf, cx, cy, nbins=36
)
theta_bins, rmse_bins_ekf, frac_ekf, rmse_global_ekf = sector_contributions(
x_gt, y_gt, x_ekf, y_ekf, cx, cy, nbins=36
)
# 4) Report top contributing sectors
def top_contrib(theta, frac, k=5):
idx = np.argsort(frac)[::-1][:k]
return [(theta[i], frac[i]) for i in idx]
print("UKF top sectors (rad, frac of SSE):", top_contrib(theta_bins, frac_ukf))
print("EKF top sectors (rad, frac of SSE):", top_contrib(theta_bins, frac_ekf))
print(f"UKF global RMSE: {rmse_global_ukf:.3f} m | EKF global RMSE: {rmse_global_ekf:.3f} m")
E) Quick polar plot (optional):
pythondef plot_polar(theta_bins, frac, title):
widths = (2*np.pi)/len(theta_bins)
ax = plt.subplot(111, projection='polar')
ax.bar(theta_bins + widths/2, frac, width=widths, align='center')
ax.set_title(title)
plt.show()
plot_polar(theta_bins, frac_ukf, "UKF sector SSE fraction")
plot_polar(theta_bins, frac_ekf, "EKF sector SSE fraction")
If one quadrant (e.g., top-left) shows a large fraction of SSE, that’s where most of your 17 cm RMSE is coming from.
If the per-bin RMSE is flat but fractions spike where you slowed down (more samples), that’s a sampling effect—not bigger instantaneous error.
This should answer “is the error concentrated in that sector?” precisely, and it removes ambiguity about what cx, cy should be in this context.
Yep — that polar chart confirms what you suspected earlier.
Almost all the RMSE energy is coming from one half of the circle, especially the top-left quadrant (around 90°–150°).
The rest of the loop has much lower contributions, which means the UKF’s positional error is not uniformly distributed — it’s concentrated in that sector.
That also explains why your overall RMSE (~0.15–0.17 m) is higher than it looks visually: the estimator is doing very well most of the time, but a few angles dominate the statistics.
If you want, I can help you:
Quantify exactly what percentage of the RMSE is from that quadrant.
Compare EKF vs UKF sector-by-sector.
Hypothesize why that quadrant is worse (sensor geometry, wheel slip, yaw bias, etc.).
It
looks like that figure is fully black — probably because the plot was
exported with a dark background but no visible grid or trace lines.
If that’s the sector contribution plot, we can still process the data
and tell exactly how much of the RMSE is from the top-left quadrant.
Do you want me to:
Run the sector contribution calculation on your UKF data and sum the SSE fraction for, say, 90°–180°?
That would give you a percentage like “X% of total RMSE comes from that sector.”
That one is also completely black, so whatever you exported lost all the plotted elements.
If you want to see sector contributions clearly, you’ll need to re-run
the polar plot generation without a dark background or with visible
grid/lines.
In your script, before plotting, you can add:
pythonplt.style.use('default') # ensures white background
fig = plt.figure(facecolor='white')
ax = fig.add_subplot(111, polar=True)
If you send me your sector contribution data (the contrib array from sector_contributions), I can tell you exactly which sector — for example, your “top-left” — dominates your RMSE and by how much.
Do you want to paste that array here?
This latest plot is also just black, so whatever you're exporting is losing all drawn elements — likely because the figure background and plot colors are both black.
If you want it to be visible, in your sector plot code you can force a white background and contrasting bars:
pythonplt.style.use("default")
fig = plt.figure(facecolor="white")
ax = fig.add_subplot(111, polar=True)
ax.bar(bins, contrib / contrib.sum(), width=(2*np.pi)/nbins, color="tab:blue", edgecolor="black")
plt.show()
If you share the numeric output of:
pythonbins, contrib, rmse_total = sector_contributions(...)
print(contrib)
I can tell you exactly which angular sector is dominating your RMSE without relying on the plot.
Do you want to paste those numbers here?
This xy_error_over_time.png
also came out as pure black — so your figure background and plot lines
are probably set to black, or your export is happening in a dark style
without inverting the background.
If you want the exported images to look like what you see in a normal Jupyter/Matplotlib interactive window, force a light theme and background before saving:
pythonplt.style.use("default") # resets to default light mode
fig.patch.set_facecolor('white')
ax = plt.gca()
ax.set_facecolor('white')
and make sure your plotted lines are in colors like "black", "blue", "red".
If you just want to figure out where most of the RMSE is coming from without fixing the plot yet, you can directly print the sorted sector contributions:
pythonbins, contrib, rmse_total = sector_contributions(...)
for angle, c in sorted(zip(bins, contrib), key=lambda x: -x[1]):
print(f"{np.degrees(angle):.1f}° : {c:.4f}")
That will give you the angular positions (relative to your circle center) where the SSE is largest.
Do you want me to adapt your code so it prints that ranking instead of making the polar plot?
It looks like your plot is indeed showing the XY position error over time — essentially the magnitude of the 2D positional difference between your ground truth and estimated trajectory at each timestamp.
If you compare it with your sector plot, you can see how the largest spikes in this time series correspond to the angular sectors (around 120°–150°) where the UKF error is highest. This means the spatial error is concentrated in specific parts of the loop, not uniformly spread over the trajectory.
If you want, I can overlay markers showing where along the circle those peaks occur so you can directly link them to the trajectory plot.
Agreed. If yaw tracks GT but the XY error oscillates between 0–0.25 m, the culprit is almost surely translation, not heading. In other words: your robot’s orientation estimate is fine, but its position integration is biased/noisy.
Speed scale bias (wheel radius / encoder ticks / track-width / steering-efficiency): correct yaw but slightly wrong path length → along-track error that waxes/wanes around the circle.
Timing mismatch between IMU and odom: tiny delay in vx makes the integrated position lead/lag while yaw still matches.
Slip in turns modeled as zero-mean but not: causes radial (in/out) error at certain quadrants with nearly perfect yaw.
SE(2) misalignment when comparing to GT: constant translation offset left in the data makes one arc look worse.
Compute radial vs tangential error w.r.t. the circle center; if yaw is good, most error will be tangential (along-track) or a repeatable radial bias in certain sectors.
Formulas (for each sample, with circle center c=(cx,cy), radius R):
r=pgt−c,r^=r/∥r∥
e=pest−pgt
Radial error: er=e⋅r^
Tangential error: et=e⋅[−r^y,r^x]
Patterns:
et ≠ 0 with near-zero er ⇒ speed scale / timing issue.
er peaks in certain quadrants ⇒ slip/steering bias in those turns.
Also check:
Path-length ratio Lest/Lgt (should be ~1.00).
Cross-correlate vx (or yaw) between GT and odom to estimate a time delay; even 20–40 ms can show as oscillatory XY error with good yaw.
Recalibrate wheel radius / ticks-to-meters and track width/steering efficiency (use your circle data: match radius & path length).
If delay found, time-shift one stream or set accurate sensor time offsets.
Increase UKF process noise on vx / steering (or slip states, if modeled) so it adapts more in high-curvature sectors.
For fair metrics, do an SE(2) alignment before RMSE.
If you want, I can give you a tiny snippet to compute er/et, path-length ratio, and a simple delay estimate so you can confirm exactly which of these is biting you.
If you want to combine your X and Y RMSE into a single positional RMSE and keep yaw separate, you can do:
RMSEpos=RMSEX2+RMSEY2Filtered odom
RMSEpos=0.11532+0.13542
= 0.01329+0.01833
= 0.03162
≈ 0.1778 m
RMSEyaw=0.0021 rad ≈ 0.12°
Raw odom
RMSEpos=0.10342+0.11232
= 0.01069+0.01261
= 0.02330
≈ 0.1527 m
RMSEyaw=0.0025 rad ≈ 0.14°
Summary table:
| Metric | Filtered Odom | Raw Odom |
|---|---|---|
| RMSE positional (m) | 0.1778 | 0.1527 |
| RMSE yaw (rad / °) | 0.0021 / 0.12° | 0.0025 / 0.14° |
So in your case:
Raw odom is slightly better in positional RMSE (~15 cm vs 17.8 cm).
Filtered odom is slightly better in yaw RMSE (~0.12° vs 0.14°).
If you want, I can also compute the % improvement or relative degradation between the two.
Here’s a tighter, paper-ready version of that paragraph:
The EKF and UKF substantially improved odometry accuracy. While grid-style navigation (e.g., squares) is often feasible, it is not always possible or efficient. In practice, the dominant error sources are slip—especially during rotation—and IMU drift. To stress the system under continuous curvature, we commanded the robot to traverse a circle of radius 10m for 10 laps. Because the local odometry relies heavily on the IMU and the tracked controller induces slip, we expected larger discrepancies on this path.
Figure~\ref{figure:circle_10_meter_fused} confirms a mismatch between ground truth and local odometry. The EKF achieves a positional RMSE of 0.1778m and a yaw RMSE of 0.12∘; the UKF slightly improves positional RMSE to 0.1527m with a yaw RMSE of 0.14∘. Notably, the estimate tends to deviate on the left-hand arc and then reconverge near the bottom of the circle. This localized deviation contributes disproportionately to the overall positional (and to a lesser extent orientational) RMSE; sector-wise error analysis highlights that quadrant as the primary source of the total SSE.
You could phrase that as:
A visual inspection of the trajectories shows that the robot’s estimate consistently reconverges with the ground truth, resulting in sufficiently accurate performance for the purposes of this test.
Or, if you want it to flow into the previous paragraph you gave:
Notably, the estimate tends to deviate on the left-hand arc and then reconverge near the bottom of the circle. A visual inspection confirms that this reconvergence enables the robot to maintain sufficiently accurate odometry for the scope of this test.
Absolutely —turn “looks like it reconverges” into evidence with a few quick, quantitative checks. Pick 2–3 of these and you’ll have a solid proof.
Return-to-start (loop-closure) error per lap
If the end of each lap is within a small distance of the start (e.g., <1–2% of lap length), that’s reconvergence.
Per-angle error profile with a minimum at the bottom
Bin errors by heading around the circle; if the mean error near the
bottom sector is consistently the lowest, that supports reconvergence.
Error slope approaching the bottom
Regress error vs. angle over the last K degrees before the bottom (e.g., 210°→270°). A significantly negative slope shows error decays as you approach the bottom.
Per-lap windowed RMSE near the bottom vs. opposite side
Compute RMSE in a ±20° window around the bottom for each lap, and
compare to a window on the opposite side (e.g., left/top). A paired test
(t-test or Wilcoxon) showing bottom-window RMSE is smaller ⇒
reconvergence.
Innovation/residual energy near zero (if you log EKF/UKF residuals)
Mean normalized innovation squared (NIS) dipping near the bottom shows the filter and sensors agree there.
This adds: (a) circle center fit, (b) lap segmentation, (c) bottom vs. left window RMSE per lap, (d) error slope before bottom, and (e) loop-closure per lap.
pythonimport numpy as np, math
from scipy.stats import ttest_rel
def fit_circle_kasa(x, y):
X = np.column_stack([x, y, np.ones_like(x)])
b = -(x**2 + y**2)
A, B, C = np.linalg.lstsq(X, b, rcond=None)[0]
cx, cy = -A/2.0, -B/2.0
r = math.sqrt(cx**2 + cy**2 - C)
return cx, cy, r
def unwrap_laps(theta, crossing_angle=-math.pi/2): # bottom at -90° (π/2 clockwise)
# Shift angles so "bottom" is the crossing at 0
th = ((theta - crossing_angle + math.pi) % (2*math.pi)) - math.pi
# Detect lap when wrapped angle jumps backward
jumps = np.where(np.diff(th) < -np.pi)[0]
# Build lap indices
starts = np.r_[0, jumps+1]
ends = np.r_[jumps, len(th)-1]
return list(zip(starts, ends)), th
def window_mask(theta_shifted, center_deg, halfwidth_deg):
c = math.radians(center_deg)
w = math.radians(halfwidth_deg)
# Wrap-aware mask around c
d = (theta_shifted - c + math.pi) % (2*math.pi) - math.pi
return np.abs(d) <= w
def analyze_reconvergence(x_gt, y_gt, x_est, y_est, nbottom=20, nleft=20):
# 1) Center & angle
cx, cy, r = fit_circle_kasa(x_gt, y_gt)
e = np.hypot(x_gt - x_est, y_gt - y_est)
theta = np.arctan2(y_gt - cy, x_gt - cx) # [-pi, pi]
laps, th_shift = unwrap_laps(theta, crossing_angle=-math.pi/2) # bottom
# 2) Per-lap RMSE in bottom vs left windows
bottom_rmse, left_rmse, loop_err = [], [], []
for s, eidx in laps:
idx = slice(s, eidx+1)
th_lap = th_shift[idx]
err_lap = e[idx]
# bottom window centered at 0°, left window at +180° (opposite bottom)
m_bottom = window_mask(th_lap, center_deg=0, halfwidth_deg=nbottom)
m_left = window_mask(th_lap, center_deg=180, halfwidth_deg=nleft)
if m_bottom.any():
bottom_rmse.append(np.sqrt(np.mean(err_lap[m_bottom]**2)))
if m_left.any():
left_rmse.append(np.sqrt(np.mean(err_lap[m_left]**2)))
# loop-closure error for this lap
loop_err.append(np.hypot(x_est[eidx]-x_est[s], y_est[eidx]-y_est[s]))
# 3) Error slope approaching bottom (e.g., last 60° before bottom)
pre_mask = window_mask(th_shift, center_deg=-30, halfwidth_deg=30) # 210°→270° region in original angle space
th_pre = th_shift[pre_mask]
e_pre = e[pre_mask]
slope = np.polyfit(th_pre, e_pre, 1)[0] if len(th_pre) > 5 else np.nan # slope < 0 ⇒ decaying error
# 4) Stats summary
stats = {}
if len(bottom_rmse) and len(left_rmse) and len(bottom_rmse) == len(left_rmse):
tstat, pval = ttest_rel(bottom_rmse, left_rmse)
stats['paired_t_pval_bottom_vs_left'] = pval
return {
'center': (cx, cy, r),
'per_lap_bottom_rmse': bottom_rmse,
'per_lap_left_rmse': left_rmse,
'per_lap_loop_closure_m': loop_err,
'pre_bottom_error_slope': slope, # negative is good (reconvergence)
'stats': stats
}
How to interpret:
Loop-closure per lap small (e.g., few cm): reconvergence each lap.
Bottom RMSE < Left RMSE across laps (and paired t-test p < 0.05): statistically supports “reconverges near the bottom.”
Pre-bottom slope < 0: error decays as you approach the bottom (objective “convergence” signal).
Drop this right after you build df_merged (for EKF and/or UKF), e.g.:
pythonres = analyze_reconvergence(df_merged["x_gt"].to_numpy(),
df_merged["y_gt"].to_numpy(),
df_merged["x_raw"].to_numpy(),
df_merged["y_raw"].to_numpy())
print(res)
This turns your visual claim into clear, testable statements you can put in the paper:
“Mean loop-closure error per lap: X m (Y% of lap length).”
“Bottom-window RMSE (mean ± std) = a ± b m vs. left window = c ± d m; paired t-test p = …”
“Error slope over last 60° before bottom: negative (s = …), indicating decay toward the bottom.”
Yes — the way you’ve described it already points strongly toward the odometry being the main source of error rather than the IMU.
Here’s why:
Yaw tracking is good
Your plots show the yaw from EKF/UKF is essentially on top of the ground truth.
If the IMU were drifting badly, yaw would wander over time, especially in 10 laps.
This means the heading input to the filter is accurate enough that IMU drift isn’t the dominant error here.
XY error grows and oscillates
The error in position rises to ~0.25 m and then oscillates even though yaw stays correct.
This pattern means the robot is pointing in the right direction but the distance traveled estimate is off — a classic sign of wheel slip, encoder bias, or other odometry error.
Circular path stress test
A circle of fixed radius and repeated laps magnifies systematic odom errors (slip, unequal wheel diameters, scaling factors).
If the IMU were the problem, you’d expect gradual heading drift, not a position drift pattern that corrects at certain points.
Reconvergence near the bottom
The filter is able to “pull” the position back in line when enough geometry and sensor fusion helps (probably from combining yaw + wheel odom).
This behavior means the system has a reliable heading reference (IMU/mag) and the main discrepancy is in how far it thinks it’s gone.
If you wanted to make this airtight, you could:
Compare the raw wheel odometry path vs. IMU-integrated position (dead-reckoned purely from IMU yaw + wheel speed magnitude).
Disable wheel odom in the filter for one run and see if drift pattern changes — if it vanishes or looks different, you’ve proven the odom is the culprit.
I can write a short “proof-style” paragraph for your paper that says this clearly if you want.
Gotcha. You can turn that XY-error-over-time plot into hard evidence with a few quick summaries that tie time → angle → “where the error comes from.” Here’s a compact recipe (drop-in code) plus what to report.
Overall stats: mean, RMSE, 95th percentile, max.
Peaks (e.g., >0.20 m) and when they occur.
Map each timestamp to the circle angle; show those peaks all sit in the same sector.
Per-lap bottom-window RMSE vs. opposite side (proves reconvergence near the bottom).
Loop-closure error per lap (small → reconverges each lap).
Add this after you build df_merged (and reuse your fit_circle_kasa from earlier if you added it; I include a minimal version here):
pythonimport numpy as np, math
from scipy.stats import ttest_rel
def fit_circle_kasa(x, y):
X = np.column_stack([x, y, np.ones_like(x)])
b = -(x**2 + y**2)
A, B, C = np.linalg.lstsq(X, b, rcond=None)[0]
cx, cy = -A/2.0, -B/2.0
r = math.sqrt(max(0.0, cx**2 + cy**2 - C))
return cx, cy, r
def unwrap_laps(theta, crossing_angle=-math.pi/2):
th = ((theta - crossing_angle + math.pi) % (2*math.pi)) - math.pi
jumps = np.where(np.diff(th) < -np.pi)[0]
starts = np.r_[0, jumps+1]
ends = np.r_[jumps, len(th)-1]
return list(zip(starts, ends)), th
def window_mask(theta_shifted, center_deg, halfwidth_deg):
c = math.radians(center_deg)
w = math.radians(halfwidth_deg)
d = (theta_shifted - c + math.pi) % (2*math.pi) - math.pi
return np.abs(d) <= w
def xy_error_series(df, which="ekf"):
if which == "ekf":
dx = df["x_gt"] - df["x_odom"]; dy = df["y_gt"] - df["y_odom"]
else:
dx = df["x_gt"] - df["x_raw"]; dy = df["y_gt"] - df["y_raw"]
return np.hypot(dx, dy)
def analyze_xy_series(df, which="ekf", peak_thresh=0.20, bottom_win=20, opp_win=20):
# 1) XY error time series
e = xy_error_series(df, which)
rmse = float(np.sqrt(np.mean(e**2)))
p95 = float(np.percentile(e, 95))
emax = float(np.max(e))
t = df["time_s"].to_numpy()
# 2) Fit circle center from GT and get angle per sample
cx, cy, r = fit_circle_kasa(df["x_gt"].to_numpy(), df["y_gt"].to_numpy())
theta = np.arctan2(df["y_gt"]-cy, df["x_gt"]-cx) # [-pi, pi]
laps, th_shift = unwrap_laps(theta, crossing_angle=-math.pi/2) # bottom crossing
# 3) Peak detection and where they land (angle)
peak_idx = np.where(e >= peak_thresh)[0]
peak_angles_deg = np.degrees(theta[peak_idx]) if len(peak_idx) else np.array([])
# 4) Per-lap: bottom vs opposite (left) RMSE; loop-closure error per lap (est path)
if which == "ekf":
xe, ye = df["x_odom"].to_numpy(), df["y_odom"].to_numpy()
else:
xe, ye = df["x_raw"].to_numpy(), df["y_raw"].to_numpy()
bottom_rmse, opp_rmse, loop_err = [], [], []
for s, eidx in laps:
idx = slice(s, eidx+1)
err_lap = e[idx]; th_lap = th_shift[idx]
m_bottom = window_mask(th_lap, center_deg=0, halfwidth_deg=bottom_win) # around bottom
m_opp = window_mask(th_lap, center_deg=180, halfwidth_deg=opp_win) # opposite side
if m_bottom.any(): bottom_rmse.append(float(np.sqrt(np.mean(err_lap[m_bottom]**2))))
if m_opp.any(): opp_rmse.append(float(np.sqrt(np.mean(err_lap[m_opp]**2))))
loop_err.append(float(np.hypot(xe[eidx]-xe[s], ye[eidx]-ye[s])))
# 5) Paired test: bottom-window vs opposite-window RMSE across laps
from scipy.stats import ttest_rel
pval = None
if len(bottom_rmse) == len(opp_rmse) and len(bottom_rmse) > 1:
_, pval = ttest_rel(bottom_rmse, opp_rmse)
return {
"rmse_total_m": rmse,
"p95_m": p95,
"max_m": emax,
"num_peaks_over_thresh": int(len(peak_idx)),
"peak_angles_deg": peak_angles_deg, # inspect concentration (e.g., top-left)
"center": (cx, cy, r),
"per_lap_bottom_rmse_m": bottom_rmse,
"per_lap_opposite_rmse_m": opp_rmse,
"per_lap_loop_closure_m": loop_err,
"paired_t_p_bottom_vs_opp": pval
}
Use it:
pythonres_ekf = analyze_xy_series(df_merged, "ekf", peak_thresh=0.20)
res_ukf = analyze_xy_series(df_merged, "ukf", peak_thresh=0.20)
print("EKF:", res_ekf)
print("UKF:", res_ukf)
“The XY error time series has RMSE =0.178m (EKF) with a 95th percentile of p95_ekf and a maximum of max_ekf.”
“Peaks above 0.20 m occur predominantly at angles θ∈[…] (top-left quadrant), confirming spatial concentration of error.”
“Per-lap loop-closure error median = e~loop m ( % of lap length), indicating reconvergence each lap.”
“Bottom-window RMSE (mean ± std) = a±b m vs. opposite side c±d m; paired t-test p= pval < 0.05 → error near the bottom is significantly smaller, consistent with reconvergence.”
This converts your XY-error plot from a visual into quantitative, defensible evidence that (1) the translation channel is the culprit (yaw is fine), and (2) the estimate reconverges near the bottom of the loop.
Here’s feedback plus a tight rewrite you can drop into the paper. I’ve also suggested a couple of small additions that make the claim more rigorous without adding new experiments.
You repeat the idea that squares may be inefficient—mention it once, then move on.
Say what “accurate enough” means (an acceptance criterion), not just that it “looks good.”
Anchor the errors to the test scale (e.g., % of radius / error per 100 m / loop-closure per lap).
Call out the localized error (left arc) with one quantitative line so it’s not just “visual inspection.”
Use consistent terms: “yaw RMSE” or “rotational RMSE,” pick one.
One sentence about methodology (SE(2) alignment, time sync) helps credibility.
To evaluate performance on continuous-curvature motion, we commanded the robot to traverse a circle of radius 10m for 10 laps (total path ≈628m). Figure~\ref{figure:circle_10_meter_fused} compares ground truth with EKF- and UKF-based odometry after time synchronization and SE(2) alignment. The EKF achieves a positional RMSE of 0.1778m and a yaw RMSE of 0.12∘. The UKF slightly reduces positional RMSE to 0.1527m with a yaw RMSE of 0.14∘. Expressed relative to scale, this is 1.5–1.8% of the circle radius and 2.4–2.8cm per 100m traveled.
Sector-wise analysis shows that errors are concentrated on the left-hand arc, while the estimate consistently reconverges near the bottom of the circle, yielding small loop-closure errors at each lap end. Despite this localized deviation, the filters maintain heading accuracy and bounded translational error, which we deem sufficient for planners that rely on circles and smooth arcs as motion primitives.
Acceptance criterion: “We consider odometry sufficient for arc-based planning when positional RMSE <2% of turn radius and loop-closure error <1% of lap length; both filters meet this criterion.”
Loop closure: “Median per-lap return-to-start error was x m (y% of lap length).”
Localization of error: “The top-left quadrant contributed z% of the total SSE, confirming the deviation is localized rather than systematic.”
Planner relevance: “These errors are smaller than the costmap inflation radius (0.25 m), so tracking of curvature-bounded paths is unaffected.”
Replace “visual inspection confirms…” with “per-sector RMSE and per-lap loop-closure confirm reconvergence near the bottom.”
Use one term for rotation: “yaw RMSE”.
Add units everywhere the first time a number appears.
If you share (or compute) the loop-closure % and sector SSE fraction, I can slot those numbers into the rewrite so it reads airtight.
That’s a good starting point, but it can be made sharper and more academic by tightening the flow and clearly framing why this experiment matters.
Here’s a refined version:
It has been shown that incorporating the EKF and UKF significantly improves the accuracy of the robot’s odometry. While grid-based navigation using square patterns is common, such traversal may be impractical or inefficient in certain environments. In these cases, the robot must negotiate obstacles using long, continuous arcs. This type of motion is more sensitive to odometric error, as most inaccuracies arise from wheel slip during rotation and IMU drift. Evaluating the filters under sustained curvature motion is therefore critical to understanding their suitability for real-world navigation.
If you want, I can help you rewrite the rest of the section so this introduction flows directly into your 10 m radius circle experiment. That would make it sound cohesive and deliberate rather than like two separate ideas.
For a tracked vehicle, travelling in a continuous circle is generally more challenging than rotating on the spot because it combines simultaneous translation and rotation, which increases the number of error sources acting at once.
Here’s why:
Unequal track slip
In a pure rotation, both tracks move in opposite directions at the same speed, so the slip pattern is symmetrical and easier to model/compensate.
In a continuous turn, the inner and outer tracks run at different speeds, so the slip rate is asymmetric and depends on ground conditions, turning radius, and load distribution.
Constant curvature → cumulative integration error
In a spot turn, you rotate to a target heading, then stop integrating motion.
In a continuous circle, the heading is constantly changing while the vehicle is also translating, so small errors in yaw estimation continually distort position.
IMU drift effects are amplified
An IMU’s yaw bias slowly drifts; during a circle, this drift directly changes the perceived turning radius. Even a small heading error will accumulate into large XY position errors.
Drive system non-linearities
Track tension, wear, and control system response differences between left and right tracks cause subtle speed mismatches that grow over distance.
In a spot turn, you only notice these as a small yaw offset. In a long turn, they create a spiral-like position error.
Surface interaction variability
Turning while moving forward produces complex ground–track interactions.
The outer track experiences more shear force (more slip), and the slip can vary with terrain patches — much harder to model than in-place turning.
So in short:
Spot turns are essentially rotational calibration checks, but continuous circles are full dynamic tests
— they stress the odometry, the IMU, and the track dynamics all at
once, making them far more challenging for maintaining positional
accuracy.
If you want, I can work this reasoning into your paper’s circle experiment section to make the difficulty — and thus the validity — of your test more obvious.
Yes — increased ground contact is another important factor for why continuous turning is harder for tracked vehicles.
When a tracked vehicle turns while moving forward:
More track length is in contact with the ground compared to in-place rotation.
This means a larger contact patch is under lateral shear stress, especially on the outer track, which increases the risk of uneven slip.
The extra contact also amplifies resistance forces (scrub forces), so any slight imbalance in torque delivery between the two tracks creates more drift in heading and curvature.
Because the slip distribution isn’t uniform along the track, it’s harder to model or compensate for in odometry — errors vary depending on terrain, speed, and turning radius.
So, continuous arcs/circles = more ground contact under stress → more complex slip dynamics → bigger challenge for accurate odometry.
If you like, I can rewrite your experimental motivation section to clearly show that this is why your 10 m radius circle test is a stress case for EKF/UKF performance.
Yes — this is a lot better than before.
It’s clearer, explains why circles are harder for tracked vehicles, and logically flows from problem → challenge → test → result → conclusion.
I’d suggest just a few tweaks for grammar, conciseness, and to make the technical points sharper:
Revised version
It has been shown that the addition of the EKF and UKF significantly improves the accuracy of the robot's odometry. While grid-based navigation—especially with tracked robots—using square patterns is a common exploratory approach, there are occasions where such a pattern is inefficient or impractical. In these cases, continuous circular or arc traversals around obstacles can be a more practical solution. However, these manoeuvres introduce additional challenges for tracked vehicles: increased ground contact amplifies resistance forces, and the need for the two tracks to run at different speeds increases the likelihood of slip. This slip can cause substantial odometry inaccuracies. Furthermore, IMU drift means even small yaw errors can accumulate into significant X/Y positional errors.
Given that circular and arc motion is an important traversal pattern, an experiment was designed to evaluate filter performance under these conditions. The robot was commanded to traverse a circle with a 10 m radius, completing 10 full loops. Due to the odometry’s heavy reliance on the IMU, coupled with controller-induced slip, it was expected that more pronounced discrepancies would appear. Figure \ref{figure:circle_10_meter_fused} shows differences between the ground truth and the robot's local odometry. The Extended Kalman Filter reported a positional RMSE of 0.1778 m and a yaw RMSE of approximately 0.12°, while the Unscented Kalman Filter achieved a slightly lower positional RMSE of 0.1527 m but a yaw RMSE of 0.14°.
Notably, both filters’ odometry deviated on the left-hand arc before reconverging near the bottom of the circle. This localised deviation contributed disproportionately to the overall positional RMSE. Visual inspection confirms that despite these deviations, the robot consistently reconverged to the path, resulting in overall accuracy sufficient to support navigation using circles and smooth arcs.
This keeps all your points but trims repetition and strengthens the justification for the test.
If you also include your XY error over time
plot, you could quantitatively back the “deviate then reconverge”
observation instead of relying only on visual inspection. That would
make the conclusion more bulletproof.
Do you want me to show you exactly how to reference that XY error graph in this text? That would tie the reasoning together neatly.
Let’s break down what you’ve already covered and where you could strengthen things:
1. Context – Why circular/arc navigation matters
You’ve touched on it briefly (“occasions where such a pattern is
inefficient or impractical”) but right now it’s still a bit abstract.
If you add 1–2 sentences explicitly connecting this to real-world tasks, it will feel stronger. For example:
“Circular and arc-based navigation is particularly important in real-world scenarios such as manoeuvring around curved obstacles, following natural terrain contours, or executing smooth turns in cluttered environments where sharp stops and pivots would be inefficient or unsafe.”
2. Importance – Why tracked vehicles struggle here
You’ve already mentioned:
Increased ground contact
Tracks moving at different speeds
Slippage
IMU drift and yaw errors affecting XY position
This is already solid. You might make it more explicit that these issues are compounded over long arcs because there is no “reset” like in stop-and-turn maneuvers — the error integrates continuously.
3. Interpretation – What the test shows and whether filters are good enough
You’ve partly done this in:
“A visual inspection confirms that while the robot does deviate, it converges and remains reasonably accurate.”
But you could strengthen the conclusion with:
A statement about operational acceptability (e.g., “The level of error observed remains well within tolerances for typical navigation and mapping tasks, suggesting the EKF and UKF are sufficient for continuous-arc path planning.”)
Linking the RMSE numbers to real-world scale (e.g., “An error of ~0.15–0.18 m over a 10 m radius circle represents only 1.5–1.8% of the total path scale.”)
If you’d like, I can rewrite your final paragraph so it:
Gives concrete context
Emphasizes why it’s important
Clearly states what the results mean in terms of the filters being “good enough.”
That would make your conclusion read like a clear “finding” section in a paper.
Do you want me to do that rewrite?
That ~2% figure comes from putting your positional RMSE in context with the scale of the motion.
Here’s the math:
Circle radius: r=10m
RMSE (EKF): 0.1778m
RMSE (UKF): 0.1527m
If you compare that to the radius of the circle (10 m):
100.1778×100≈1.78%(EKF) 100.1527×100≈1.53%(UKF)So when you say “RMSE is ~0.15–0.18 m”, you’re really saying the robot’s positional deviation is only about 1.5–1.8% of the path scale, which sounds much smaller and more acceptable in practical navigation terms.
That percentage can be useful in your conclusion because it frames the absolute RMSE in a relative, intuitive way.
If you want, I can rewrite your conclusion to include that context so the reader immediately sees how small the error is relative to the path size.
Here’s how you could continue and tighten that opening so it flows into your stress-test explanation while keeping the technical context clear:
An exploratory grid search using squares with obstacle avoidance, combined with path optimisation using circles and arcs, is likely a suitable traversal strategy for most environments. Even tight paths, such as those in a zig-zag pattern, can be achieved using the aforementioned approaches with a reasonable level of accuracy through a series of stop-starts.
That said, to evaluate performance under a more demanding and continuous motion pattern, a stress test was devised in which the robot was commanded to traverse a circle with a 10 m radius for 10 full rotations. This scenario was chosen because continuous arcs—particularly for tracked vehicles—introduce additional slip due to the differing track speeds and increased ground contact, amplifying odometry drift. The test therefore represents a worst-case, sustained-motion challenge for the localisation filters.
If you want, I can finish the rest of the section in this style so it flows smoothly into your results and conclusion. That way, the context, importance, and interpretation are all addressed in one coherent narrative.
Ah — if this is for your S-trajectory rather than the circle test, then you can emphasise why the S-shape is even harder than a pure circle, while still keeping the same structured flow.
Here’s a rewrite for that:
An exploratory grid search using squares with obstacle avoidance, combined with path optimisation using circles and arcs, is likely a suitable traversal strategy for most environments. Even tight paths, such as those in a zig-zag pattern, can be achieved using the aforementioned approaches with a reasonable level of accuracy through a series of stop-starts.
That said, to evaluate performance under a more demanding and realistic motion pattern, a stress test was devised in which the robot followed an S-shaped trajectory. This path is significantly harder than a constant-radius circle because it requires continuous curvature changes— switching between left and right turns—which amplifies slip on tracked vehicles. Each curve reversal forces one track to decelerate while the other accelerates, producing transient yaw errors and compounding odometry drift. Additionally, increased ground contact friction and the coupling between IMU drift and wheel slip mean that positional errors can accumulate faster than in steady-state turning.
This test therefore represents a worst-case, dynamic traversal scenario, challenging the localisation filters’ ability to handle complex, alternating-arc motion while maintaining accurate pose estimation.
If you want, I can now help you adapt the circle test analysis language to this S-trajectory so that your results read consistently across both experiments. That would make it easier to compare difficulty and filter performance directly.
The main reason S-trajectories are harder for tracked vehicles than for wheeled robots comes down to how they generate turning and how slip accumulates:
Wheeled robots (especially with steering axles or differential drive with high-friction tyres) pivot by rolling without necessarily scrubbing much against the ground.
Tracked vehicles turn by driving one track faster than the other (or reversing one track), which means part of the track is always sliding sideways against the ground. This scrub generates higher resistance and more uneven slip.
In an S-curve, you aren’t just holding a constant turning rate (as in a circle).
You’re continuously switching from left to right turn.
Each reversal causes one track to decelerate and reverse relative slip direction, which increases transient yaw error and can temporarily throw off odometry alignment.
In a circle, slip tends to be systematic—it’s consistent and can sometimes be partially “averaged out” by the filter.
In an S-curve, slip is non-uniform— you get bursts of slip in opposite directions. This variation is much harder for a filter to model, so errors can grow unpredictably.
The IMU measures rotational changes well over short time spans, but in an S-path the yaw bias alternates sign due to direction changes.
This alternation can reveal IMU drift that might be masked in steady turning, since the filter is constantly recalibrating between left and right arcs.
Tracks have a much larger ground contact area than wheels.
This makes them more stable in straight lines, but when constantly changing curve direction, the increased friction means more force is needed for steering — and therefore more slip when doing quick reversals.
If you like, I can help you write a short, clean justification for your paper that explains why the S-trajectory is a tougher test for tracked robots while still being manageable for wheeled platforms. That way, it’s clear you’re not just making the test harder arbitrarily — it’s mechanically justified.
Here’s a cleaner, tighter version you can drop in, plus a couple of optional one-liners to make it stronger.
Rewritten paragraph (S-trajectory stress test)
An exploratory grid search using squares with obstacle avoidance, combined with path optimisation using circles and arcs, is likely a suitable traversal strategy for most environments. Even tight, zig-zag paths can be executed with reasonable accuracy via stop–start manoeuvres. To evaluate filter performance on a more demanding, non-linear path, we devised an S-trajectory stress test in which the robot oscillates left and right with varying periods. Unlike a constant-radius circle, this path imposes continuous curvature reversals: the inner and outer tracks alternately accelerate and decelerate, increasing scrub forces and asymmetric slip. These oscillations amplify the drift already observed in arc motion, while frequent sign changes in yaw rate expose any IMU bias that might be masked during steady turning. The result is a trajectory that stresses both the translational (distance) and rotational (heading) channels simultaneously, providing a stringent assessment of the EKF/UKF under realistic, alternating-arc motion.
If you want to strengthen it further, add one or two of these:
Why it matters (operational):
“S-like manoeuvres occur naturally when weaving through clutter,
following winding trails, or contouring around large obstacles where
stop–turn–go is inefficient or unsafe.”
Expectation & finding (short):
“We expected larger positional discrepancies during curvature reversals;
empirically, errors concentrate near turn transitions, while the
estimate reconverges on straights and gentle arcs.”
Acceptance criterion (make ‘accurate enough’ concrete):
“For arc-based planning we consider odometry sufficient when positional RMSE < 2% of the local path scale and per-segment heading RMSE < 0.2°; both filters met these thresholds on the S-test.”
Minimal quantitative evidence to cite (easy to compute):
“Peak XY error occurs within ±k° of each reversal; bottom/straight segments show x% lower RMSE than reversal segments; per-cycle loop-closure error remains < y% of segment length.”
If you’d like, I can tailor a one-sentence results line once you share the S-trajectory RMSE (positional + yaw) and either (a) per-reversal peak error or (b) straight-segment vs. reversal-segment RMSE.
An S-shaped path exposes IMU bias because it keeps forcing the robot to change turn direction, which prevents any bias from “averaging out” the way it sometimes can during a constant turn.
Here’s why in detail:
If your IMU has a small bias in angular velocity (say, +0.05 °/s), the error it introduces is systematic.
Over the course of the circle, the filter can partially absorb that bias by blending in position updates — the bias produces a predictable, steady drift that’s easier for the EKF/UKF to model.
The robot turns left, then right, then left again — yaw rate changes sign repeatedly.
A bias that is constant in the sensor now appears as a relative shift in both turn directions:
When turning left, a +0.05 °/s bias makes the turn slightly faster than commanded.
When turning right, the same +0.05 °/s bias makes the turn slower than commanded.
This alternation means the bias no longer produces a consistent offset; instead, it creates asymmetric curvature — the left arcs will be tighter, the right arcs wider (or vice versa).
This effect accumulates both ways and can’t cancel out, so errors stack up faster than in a uniform turn.
The EKF/UKF sees alternating slip + alternating yaw bias → odometry predictions keep overshooting in one direction and undershooting in the other.
The motion model struggles because the errors don’t follow a simple, steady pattern — they flip every half-period.
The result: larger XY drift and heading inconsistencies that position updates must constantly correct.
If you want, I can work this into your stress-test write-up as a short, readable sentence like:
“Unlike a constant-radius circle, the S-trajectory forces frequent curvature reversals; a constant IMU yaw bias then produces asymmetrical turning errors that cannot average out, accelerating XY drift.”
Do you want me to slot that into your current paragraph?
That yaw error plot is exactly the kind of thing that supports the “IMU bias is exposed in S-trajectories” explanation.
Here’s what it’s showing:
You can see step-like patterns in the yaw error that align with each directional change of the S-path.
The error isn’t drifting in one direction (as in a circle test) — instead, it flips sign or magnitude each time the turn direction changes.
This is consistent with a small constant bias in yaw rate:
When turning one way, the bias slightly overestimates yaw → positive error.
When turning the other way, the same bias underestimates yaw → negative error.
Because these errors alternate, the filter is forced to make frequent corrections instead of gradually compensating, which amplifies XY drift.
If you pair this plot with your XY error-over-time graph, you could clearly argue:
The yaw error shows alternating bias from turn reversals.
The alternating bias drives the oscillating XY drift seen in the positional error plot.
If you want, I can write a short addition to your stress-test paragraph that ties this yaw plot directly to why the S-path is harder than a circle.
Do you want me to do that?
Your paragraph is already quite strong, but I think we can make it sharper by:
Clarifying why the S-path is more demanding than the circle in terms of IMU bias exposure and odometry slip.
Linking the yaw bias to your yaw error plot so it’s clear you have empirical evidence.
Making the nonlinear filter motivation explicit so the reader sees why this test is a good discriminator between EKF and UKF.
Here’s a refined version:
An exploratory grid search using squares with obstacle avoidance and path optimisation using circles and arcs is likely a suitable traversal strategy for most environments. Even tight paths similar to a zig-zag pattern can be achieved with reasonable accuracy through a series of stop–starts. However, to evaluate the performance of the filters under a more demanding and highly nonlinear path, a stress test was devised in which the robot traversed a path while oscillating left and right at varying periods. Compared to the smooth, continuous curvature of the circle test, the S-path demands rapid reversals in turn direction. For a tracked vehicle, this increases ground contact resistance, exaggerates differences in track speed, and causes repeated slip events. More critically, any small constant yaw-rate bias in the IMU is alternately over- and under-estimated during each turn reversal. This alternating bias manifests as step-like patterns in the yaw error plot, which over time compound into significant XY positional drift. While previous tests contained some nonlinear aspects, both EKF and UKF outputs remained similar, suggesting that the UKF’s nonlinear handling was not being fully exercised. The S-path introduces strong, repeated nonlinearities in both motion and error propagation, making it a fairer and more revealing comparison between the two filters.
If you add your yaw error plot immediately after this paragraph, it will visually back up the yaw bias claim.
Do you want me to also rewrite your circle experiment description so it flows naturally into this S-path test? That way the reader sees it as a logical progression.
Yes — if you haven’t actually run the S-path yet and don’t have the yaw error plot for it, you should phrase the yaw bias as an expectation or hypothesis rather than a proven result.
That way you avoid giving the impression it’s confirmed data, while still explaining why you expect it to occur.
Here’s how you could reword that section:
…For a tracked vehicle, this increases ground contact resistance, exaggerates differences in track speed, and causes repeated slip events. It is expected that any small constant yaw-rate bias in the IMU will be alternately over- and under-estimated during each turn reversal. This alternating bias would likely manifest as step-like patterns in the yaw error over time, which could compound into significant XY positional drift.
This way, you’re making it clear to the reader that this is a predicted behaviour, not yet a measured one.
If you want, I can update your entire S-path paragraph so it’s framed entirely as an experimental hypothesis leading into a test. That would make it read very cleanly.
Yaw-rate bias is basically the constant offset error in your IMU’s gyroscope measurement of rotational velocity about the vertical axis (yaw).
Here’s the breakdown in plain terms:
Your IMU’s gyroscope measures yaw rate (how fast the robot is turning left or right).
In a perfect world, if the robot isn’t rotating, the yaw rate reading would be exactly zero.
In reality, sensors have small offsets — for example, the IMU might always read +0.005 rad/s even when the robot is still.
That’s the yaw-rate bias — a fixed “baseline” error in the yaw rate measurement.
Why it matters:
Gyroscopes don’t measure angle directly — they measure rate of rotation over time, and you integrate that to get yaw angle.
If your yaw-rate reading is always slightly off (bias), the integration will accumulate this error, producing a drifting yaw angle even if the robot is perfectly stationary.
Over long arcs, circles, or oscillations (like your S-test), the effect compounds and shifts the estimated heading.
A heading shift translates into large XY errors because position integration assumes the heading is correct.
For tracked vehicles, this is worse because:
Small heading errors cause disproportionate slip or curvature changes.
Any actual slip might “mask” or “feed” the bias, making it harder for the filter to correct without external references.
If you like, I can draw a quick visual explanation showing how a small yaw-rate bias leads to big XY position drift over time. Would that help?
Short answer: you can’t eliminate yaw-rate bias forever, but you can estimate, compensate, and bound it so its effect is negligible for your runs.
Startup bias calibration (static): keep the robot still for 5–10 s, average gyro ωz, subtract that offset from all future readings. Repeat after big temperature changes.
Temperature compensation: log ωz bias vs. IMU temperature; fit a curve/table and subtract bz(T). Many IMUs publish temperature for this reason.
Online bias estimation (best): add a bias state to your filter and estimate it continuously:
ψ˙=ωz−bz,b˙z∼N(0,Qb)Measurements (mag yaw, wheel/track yaw, visual/Yaw) constrain ψ; the innovation adjusts bz. Keep Qb small so the bias changes slowly. (robot_localization doesn’t expose bias states directly—you’d add a small wrapper UKF/EKF or prefilter that outputs bias-corrected IMU.)
Zero-rate / zero-velocity updates (ZARU/ZUPT): when you detect the platform is stationary (encoders≈0, IMU accel≈g), set ωz=0 and/or nudge bz toward the measured offset.
Absolute heading to bound drift: fuse mag yaw (with conservative covariance) so long-term drift is corrected while gyro handles short-term motion. This is what you’re already doing; it keeps bias from accumulating unboundedly.
Sync & scale sanity: timestamp misalignment between gyro and odom can look like bias. Verify time sync and the gyro scale factor (deg/s↔rad/s).
Mechanical hygiene: rigid mount, vibration damping, cable strain relief; vibration can create apparent bias.
Bias isn’t constant. It changes with temperature, vibration, and time (bias instability / random walk).
With (1)+(5) you’ll greatly reduce drift for short runs.
With (3) you can keep yaw bias tracked online, so even long S-curves stay bounded.
Do a 10 s still start → subtract mean ωz.
Fuse mag yaw at ~(5∘)2 variance for long-term correction.
Add a tiny bias estimator: include bz with Qb≈(0.001 rad/s)2 and let the filter learn it.
Re-zero when your temperature changes by >10 °C or after hard impacts.
You won’t remove yaw-rate bias permanently, but you can make it irrelevant for your trajectories by calibrating at start, constraining it with absolute yaw, and (ideally) estimating bz online.
Yes — in that trajectory plot, the EKF (blue) and UKF (red dashed) are essentially overlapping, so they’re producing almost identical odometry estimates.
That usually means one (or more) of the following is happening:
The system is nearly linear in this motion, so the UKF’s advantage in handling strong nonlinearities isn’t being exercised.
An S-pattern with small yaw angles and smooth changes can still be well-approximated by the EKF linearization.
The noise models are the same, so both filters are getting similar measurement updates and process predictions.
The state transition function (your motion model) may be simple enough that higher-order sigma point estimation (UKF) doesn’t yield noticeably better results.
Data sources are strongly constraining the system (e.g., wheel odom + IMU fused tightly), so there’s little room for the UKF to diverge from the EKF.
If you want to see differences emerge, you’d need a scenario with:
Large, fast yaw changes combined with imperfect odom (forcing the filter to cope with more nonlinearity).
Noisy, nonlinear measurements (like range-bearing to beacons, or camera-based landmark detection).
Highly curved, asymmetric motions that make the linearization errors of the EKF more apparent.
From your earlier message, it sounds like your “stress test” goal for this S-trajectory was partly to create that situation — but so far the motion and noise may still be tame enough for EKF and UKF to perform the same.
If you want, I can suggest how to tweak the test so the UKF’s benefits become clearer.
Fair call. Your S run is harder than a circle, but it’s still not “stressful enough” to separate EKF vs UKF — the plot shows both filters tracking almost identically. That means the motion + noise you’re feeding them are still close to linear + well-behaved, so EKF’s first-order linearization is good enough.
Here’s how to turn it into a real stress test (and how to tell it’s working):
Bigger curvature reversals: increase S amplitude and shorten the wavelength so yaw rate changes sign faster and harder.
Aggressive speed profiles: add bursts (e.g., +50% vx on entries/exits), and brief stops (ZUPT opportunities).
Compound turns: stitch arcs of different radii (R₁→R₂→R₁) instead of a smooth sine.
Slip injections: temporarily scale one track’s speed (e.g., inner track ×0.85 for 1–2 s) at each reversal.
Steering efficiency drift: vary it slowly over time (e.g., 0.55→0.50→0.55 across the run).
Small time offset: add 20–40 ms latency to odom or IMU (and keep the other on-time).
Intermittent yaw reference: drop mag-yaw for a few seconds each half-period so the filter leans on the process model.
Inflate/deflate R on the fly: during reversals, increase odom yaw covariance 3–5× (mimic worse wheel contact), then reduce on straights.
Bias steps: inject a small gyro yaw-rate bias step (e.g., +0.01 rad/s) mid-run.
Per-reversal RMSE: window ±15–30° around each curvature sign change; report mean and 95th percentile (should spike there).
Innovation/NIS: log normalized innovation squared; expect NIS > 1 around reversals and back near 1 on straights.
Loop-closure drift per cycle: distance between successive S “nodes” (crossings); should grow when you remove mag and improve when it returns.
EKF–UKF delta: plot ‖x_EKF−x_UKF‖ over time; a real stress test will show clear separations during reversals.
Strong nonlinear process/measurement (fast yaw sign flips, slip bursts).
State-dependent noise (covariances changing with curvature/speed).
Intermittent absolute references (mag outages), forcing reliance on the nonlinear prediction.
Right now, your S is likely smooth, with steady speed and constant covariances — that’s why both filters look the same. Dial up two or three items above (my go-to combo: shorter S wavelength + slip injection + intermittent mag) and you’ll get a decisive, publishable separation.
Totally — here’s a drop-in ROS 2 node that makes your S-trajectory much more nonlinear and “stressful.” It adds: rapid curvature reversals, varying segment lengths, speed bursts, brief stops (ZUPT), and optional command latency (to mimic actuation/timestamp mismatch). These are exactly the conditions where UKF should start to separate from EKF.
python#!/usr/bin/env python3
import math
import random
import collections
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
class StressSPathDriver(Node):
"""
Nonlinear S-trajectory driver to stress EKF/UKF.
Features:
- Alternating curvature with *variable* period and amplitude
- Aggressive reversals (curvature pulses at zero crossings)
- Forward-speed bursts and dips
- Brief full stops (ZUPT opportunities)
- Optional command latency to emulate timing mismatch
"""
def __init__(self):
super().__init__('stress_s_path_driver')
self.pub = self.create_publisher(Twist, '/model/mmtr/cmd_vel', 20)
# ---- Tunables (declare as ROS params) ----
self.declare_parameter('base_speed', 0.30) # m/s (nominal)
self.declare_parameter('turn_rate_max', 0.9) # rad/s peak
self.declare_parameter('period_min', 2.0) # s (shortest half-wave)
self.declare_parameter('period_max', 4.0) # s (longest half-wave)
self.declare_parameter('cycles', 10) # how many half-waves
self.declare_parameter('burst_gain', 0.60) # 0..1 speed burst amplitude
self.declare_parameter('burst_prob', 0.25) # chance a half-wave has bursts
self.declare_parameter('stop_every_n', 3) # insert a 0.4 s stop every N half-waves (0=disable)
self.declare_parameter('stop_duration', 0.4) # s
self.declare_parameter('latency_ms', 0) # emulate command latency (0=off)
self.declare_parameter('kick_gain', 1.75) # multiplier at curvature reversal
self.declare_parameter('rate_hz', 30.0)
self.v0 = float(self.get_parameter('base_speed').value)
self.w_max = float(self.get_parameter('turn_rate_max').value)
self.T_min = float(self.get_parameter('period_min').value)
self.T_max = float(self.get_parameter('period_max').value)
self.cycles = int(self.get_parameter('cycles').value)
self.burst_gain = float(self.get_parameter('burst_gain').value)
self.burst_prob = float(self.get_parameter('burst_prob').value)
self.stop_every = int(self.get_parameter('stop_every_n').value)
self.stop_dur = float(self.get_parameter('stop_duration').value)
self.latency_ms = int(self.get_parameter('latency_ms').value)
self.kick_gain = float(self.get_parameter('kick_gain').value)
self.rate_hz = float(self.get_parameter('rate_hz').value)
# Build a schedule of half-waves with random periods and options
rng = random.Random(42)
self.halfwaves = []
for i in range(self.cycles):
T = rng.uniform(self.T_min, self.T_max)
# randomly add speed bursts to some half-waves
with_burst = rng.random() < self.burst_prob
# alternate sign of curvature (S-shape)
sgn = +1.0 if i % 2 == 0 else -1.0
self.halfwaves.append({'T': T, 'sgn': sgn, 'burst': with_burst})
# state
self.hw_idx = 0
self.hw_start = self.get_clock().now()
self.last_phase_sign = 0
self.in_stop = False
self.stop_until = None
# latency emulator (queue old commands)
self.latency_queue = collections.deque()
self.delay_ticks = max(0, int(self.latency_ms * 1e-3 * self.rate_hz))
self.timer = self.create_timer(1.0 / self.rate_hz, self.on_tick)
self.get_logger().info(
f"Stress S driver: v0={self.v0} m/s, w_max={self.w_max} rad/s, "
f"T∈[{self.T_min},{self.T_max}] s, cycles={self.cycles}, "
f"bursts(p={self.burst_prob}, gain={self.burst_gain}), "
f"stop_every={self.stop_every} (dur={self.stop_dur}s), "
f"latency={self.latency_ms} ms, kick_gain={self.kick_gain}"
)
# ---------- Helpers ----------
def now_s(self):
return self.get_clock().now().nanoseconds * 1e-9
def current_hw_elapsed(self):
return (self.get_clock().now() - self.hw_start).nanoseconds * 1e-9
def switch_halfwave(self):
self.hw_idx += 1
if self.hw_idx < len(self.halfwaves):
self.hw_start = self.get_clock().now()
self.last_phase_sign = 0
else:
# finished – publish zero and stop timer
self.pub.publish(Twist())
self.timer.cancel()
self.get_logger().info("Stress S driver complete.")
# ---------- Main ----------
def on_tick(self):
# complete?
if self.hw_idx >= len(self.halfwaves):
return
hw = self.halfwaves[self.hw_idx]
T = hw['T']
sgn = hw['sgn']
t = self.current_hw_elapsed()
# Handle inserted full stop (ZUPT)
if self.stop_every > 0 and ((self.hw_idx + 1) % self.stop_every == 0):
if not self.in_stop:
# start stop at the mid-point of the half-wave
if t >= 0.5 * T and self.stop_until is None:
self.in_stop = True
self.stop_until = self.now_s() + self.stop_dur
else:
if self.now_s() < self.stop_until:
self.publish_with_latency(0.0, 0.0) # full stop
return
else:
# end stop, resume; shift hw_start to keep phase continuous
self.in_stop = False
self.stop_until = None
self.hw_start = self.get_clock().now() # re-zero elapsed for stability
t = 0.0
# phase 0..π for each half-wave
phi = math.pi * (t / T)
if phi >= math.pi:
self.switch_halfwave()
return
# Base angular command as a sine (smooth into/out of reversal)
w_cmd = sgn * self.w_max * math.sin(phi)
# Aggressive reversal "kick" (sharp curvature change) near phi≈0,π
# window around reversal: |phi - 0|<0.15 or |phi - π|<0.15
near_rev = (phi < 0.15) or (abs(phi - math.pi) < 0.15)
if near_rev:
w_cmd *= self.kick_gain
# Forward speed with optional bursts/dips in this half-wave
v_cmd = self.v0
if hw['burst']:
# speed modulation at twice the turn frequency, so it peaks near mid-arc
v_cmd *= 1.0 + self.burst_gain * math.sin(2.0 * phi)
# Small random jitter (process noise-like)
v_cmd += 0.02 * (random.random() - 0.5) # ±1 cm/s
w_cmd += 0.05 * (random.random() - 0.5) # ±0.025 rad/s
# Clamp to reasonable limits
v_cmd = max(0.0, min(v_cmd, 1.2 * self.v0))
w_cmd = max(-1.5 * self.w_max, min(w_cmd, 1.5 * self.w_max))
self.publish_with_latency(v_cmd, w_cmd)
def publish_with_latency(self, v, w):
# push current command
self.latency_queue.append((v, w))
if len(self.latency_queue) <= self.delay_ticks:
# until buffer fills, publish zeros (acts like dropout/lag)
twist = Twist()
else:
# pop delayed command
v_pub, w_pub = self.latency_queue.popleft()
twist = Twist()
twist.linear.x = v_pub
twist.angular.z = w_pub
self.pub.publish(twist)
def main(args=None):
rclpy.init(args=args)
node = StressSPathDriver()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
Curvature reversals + kicks: strong nonlinearity right where EKF linearization struggles; UKF should handle better.
Variable periods/amplitudes: breaks any steady-state assumptions; exposes model mismatch.
Speed bursts & dips: couple translational/rotational errors; drive along-track vs radial error.
Stops (ZUPT): give the filters a chance to re-zero; you can compare recovery behaviour.
Command latency: tiny timing mismatch is notorious for making EKF/UKF diverge differently.
Start tame: turn_rate_max=0.7, kick_gain=1.3, latency_ms=0, burst_gain=0.4.
Then ramp to w_max≈1.0, kick_gain≈1.8, latency_ms≈30.
While testing, log EKF–UKF difference, per-reversal RMSE, and NIS; you should see separations at reversals.
If you want a version that periodically disables mag yaw (to force reliance on the process model), I can add a simple service/param toggler that you can hook into your mag-yaw publisher.
Ah, got it—yours is only curving counter-clockwise instead of doing left↔right oscillations. Two likely causes:
Sign never flips effectively. In my sine version, sin(ϕ) is always positive on each half-wave and we multiply by sgn
(+/–). If the half-wave never completes (e.g., because of stops/latency
resetting time), you might stay in the “+” half forever → CCW only.
Frame/sign convention. In your sim, +Z yaw might be CCW; if the sign flip logic isn’t actually reached (or clamped), you’ll see a single direction.
To make this bulletproof, here’s a simpler, deterministic “hard-S” driver that guarantees alternation by flipping the sign on a timer (no phase/sine dependence). It also keeps the stressors: short/long segments, reversal “kicks”, speed bursts, optional stops, and optional command latency.
python#!/usr/bin/env python3
import math, random, collections
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
class HardSDriver(Node):
"""
Deterministic, stressy S-trajectory:
- Piecewise-constant angular rate with guaranteed sign flips
- Randomized segment durations (short/long)
- Reversal "kick" right after each flip
- Speed bursts within segments
- Optional brief stops (ZUPT)
- Optional command latency emulation
"""
def __init__(self):
super().__init__('hard_s_driver')
self.pub = self.create_publisher(Twist, '/model/mmtr/cmd_vel', 20)
# ---- Parameters ----
self.declare_parameter('v_base', 0.30) # m/s
self.declare_parameter('w_mag', 0.9) # rad/s (magnitude)
self.declare_parameter('seg_min', 1.2) # s
self.declare_parameter('seg_max', 2.5) # s
self.declare_parameter('segments', 16) # number of segments
self.declare_parameter('kick_gain', 1.6) # multiply |w| for first kick_dur
self.declare_parameter('kick_dur', 0.20) # s duration of kick
self.declare_parameter('burst_gain', 0.5) # speed burst amplitude (0..1)
self.declare_parameter('burst_freq_hz', 1.0) # within-segment burst frequency
self.declare_parameter('stop_every_n', 0) # insert a stop every N segments (0=off)
self.declare_parameter('stop_dur', 0.4) # s
self.declare_parameter('latency_ms', 0) # emulate cmd latency
self.declare_parameter('rate_hz', 30.0)
p = self.get_parameter
self.v_base = float(p('v_base').value)
self.w_mag = float(p('w_mag').value)
self.seg_min= float(p('seg_min').value)
self.seg_max= float(p('seg_max').value)
self.nseg = int(p('segments').value)
self.kick_g = float(p('kick_gain').value)
self.kick_t = float(p('kick_dur').value)
self.burst_g= float(p('burst_gain').value)
self.burst_f= float(p('burst_freq_hz').value)
self.stop_N = int(p('stop_every_n').value)
self.stop_t = float(p('stop_dur').value)
self.lat_ms = int(p('latency_ms').value)
self.rate = float(p('rate_hz').value)
# Build schedule with guaranteed sign flip each segment
rng = random.Random(123)
self.schedule = []
sgn = +1.0
for i in range(self.nseg):
dur = rng.uniform(self.seg_min, self.seg_max)
self.schedule.append({'dur': dur, 'sgn': sgn})
sgn *= -1.0 # GUARANTEED flip
# State
self.seg_idx = 0
self.seg_start = self.get_clock().now()
self.in_stop = False
self.stop_until = 0.0
# Latency buffer
self.delay_ticks = max(0, int(self.lat_ms * 1e-3 * self.rate))
self.lat_buf = collections.deque()
self.timer = self.create_timer(1.0/self.rate, self.tick)
self.get_logger().info(
f"HardS: v={self.v_base} w={self.w_mag} segs={self.nseg} "
f"dur=[{self.seg_min},{self.seg_max}] kick={self.kick_g}x/{self.kick_t}s "
f"bursts={self.burst_g}@{self.burst_f}Hz stop_every={self.stop_N} "
f"latency={self.lat_ms}ms"
)
def now_s(self):
return self.get_clock().now().nanoseconds * 1e-9
def seg_elapsed(self):
return (self.get_clock().now() - self.seg_start).nanoseconds * 1e-9
def next_segment(self):
self.seg_idx += 1
self.seg_start = self.get_clock().now()
self.in_stop = False
self.stop_until = 0.0
if self.seg_idx >= len(self.schedule):
self.pub.publish(Twist())
self.timer.cancel()
self.get_logger().info("HardS complete.")
def tick(self):
if self.seg_idx >= len(self.schedule):
return
seg = self.schedule[self.seg_idx]
t = self.seg_elapsed()
# Optional inserted stop mid-segment
if self.stop_N > 0 and ((self.seg_idx+1) % self.stop_N == 0):
if not self.in_stop and t > seg['dur']*0.5:
self.in_stop = True
self.stop_until = self.now_s() + self.stop_t
if self.in_stop:
if self.now_s() < self.stop_until:
self.publish_with_latency(0.0, 0.0)
return
else:
# resume, but keep the remaining time of this segment
self.in_stop = False
if t >= seg['dur']:
self.next_segment()
return
# Angular rate: constant magnitude with sign for this segment
w = seg['sgn'] * self.w_mag
# Reversal kick at start of segment
if t < self.kick_t:
w *= self.kick_g
# Forward speed with within-segment burst (sine)
v = self.v_base * (1.0 + self.burst_g * math.sin(2*math.pi*self.burst_f*t))
# Small jitter
v += 0.02 * (random.random() - 0.5)
w += 0.05 * (random.random() - 0.5)
# Clamp
v = max(0.0, min(v, 1.3*self.v_base))
w = max(-1.5*self.w_mag, min(w, 1.5*self.w_mag))
self.publish_with_latency(v, w)
def publish_with_latency(self, v, w):
self.lat_buf.append((v, w))
if len(self.lat_buf) <= self.delay_ticks:
cmd = Twist() # zeros until delay buffer fills
else:
v_pub, w_pub = self.lat_buf.popleft()
cmd = Twist()
cmd.linear.x = v_pub
cmd.angular.z = w_pub
self.pub.publish(cmd)
def main(args=None):
rclpy.init(args=args)
node = HardSDriver()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
The sign flips every segment (sgn *= -1.0) regardless of pauses, kicks, or latency.
No hidden phase logic: each segment is independent, so you can’t get “stuck” CCW.
Verify your frame: in ROS/Gazebo, +angular.z is CCW. If your vehicle’s kinematics invert that, swap the sign (set w_mag negative or flip sgn seed to -1.0).
Try shorter segments: seg_min=0.8, seg_max=1.6, w_mag=1.0, kick_gain=1.8, latency_ms=25 to really stress it.
This should give you a clear S path with aggressive reversals and the stressors that tend to separate EKF and UKF.
Gotcha—if it does one clear “S” and then settles into a steady single-direction circle, that usually means the sign flip you think you’re sending isn’t actually reaching the tracks (or it’s being neutralized). Here’s how to fix + debug fast.
No real sign flip at the topic (bug in timing/index math or a stale Twist keeps being replayed).
Plugin saturation/asymmetry: inner/outer track limits or “steering_efficiency” make negative angular.z ineffective at your chosen v (so both signs turn CCW).
Latency buffer feeding old commands after the flip.
Another node/controller also publishing to /model/mmtr/cmd_vel, overwriting your reversals.
This version flips purely by elapsed time → no phase/ramp tricks; adds a brief zero between flips to kill residual motion.
python#!/usr/bin/env python3
import math, rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
class FlipS(Node):
def __init__(self):
super().__init__("flip_s")
self.pub = self.create_publisher(Twist, "/model/mmtr/cmd_vel", 10)
# params
self.declare_parameter("v", 0.30) # m/s
self.declare_parameter("w", 0.9) # rad/s (magnitude)
self.declare_parameter("seg_dur", 1.6) # s per half-wave
self.declare_parameter("dwell", 0.20) # s zero command at each flip
self.declare_parameter("segments", 20) # total half-waves
self.declare_parameter("rate_hz", 30.0)
p = self.get_parameter
self.v = float(p("v").value)
self.wmag = float(p("w").value)
self.seg_dur = float(p("seg_dur").value)
self.dwell = float(p("dwell").value)
self.nseg = int(p("segments").value)
self.hz = float(p("rate_hz").value)
self.t0 = self.get_clock().now()
self.timer = self.create_timer(1.0/self.hz, self.tick)
def t(self): # seconds since start
return (self.get_clock().now() - self.t0).nanoseconds * 1e-9
def tick(self):
t = self.t()
k = int(t // self.seg_dur) # which half-wave we are in
if k >= self.nseg:
self.pub.publish(Twist()) # stop
self.timer.cancel()
return
t_in = t - k * self.seg_dur # time within segment
sign = +1.0 if (k % 2 == 0) else -1.0
cmd = Twist()
if t_in < self.dwell: # brief zero to “reset” dynamics
pass # keep zeros
else:
cmd.linear.x = self.v
cmd.angular.z = sign * self.wmag
self.pub.publish(cmd)
# Optional: print to verify actual flips
if abs(t_in - self.dwell) < 1.0/self.hz:
self.get_logger().info(f"Flip #{k} -> sign {int(sign)}")
def main():
rclpy.init(); rclpy.spin(FlipS()); rclpy.shutdown()
if __name__ == "__main__": main()
Verify the command stream:
ros2 topic echo /model/mmtr/cmd_vel -n 40 → confirm angular.z alternates +w / -w every seg_dur.
Verify the robot’s reported yaw rate: echo odom/imu yaw rate and check sign flips. If commands flip but yaw rate doesn’t, it’s a plugin/saturation issue.
Lower v or w temporarily: e.g., v=0.2, w=0.8. If flips suddenly work, you were saturating the inner track.
Increase dwell to 0.4 s: gives the tracks time to de-saturate before reversing.
Make sure nothing else publishes to /model/mmtr/cmd_vel (use ros2 topic info … publishers list).
Steering efficiency bias: if it’s very low (e.g., 0.5), the reverse turn might be too weak; bump w for the negative segment only (or temporarily set efficiency closer to 1.0 to test).
Once you see true left↔right alternation in both the command and the measured yaw rate, you’ll get a real S instead of a circle—and the run will be a proper stress test.
Totally expected. With encoders + IMU, a smooth controller, and Gaussian-ish noise, an EKF and a UKF will often produce nearly identical odometry. robot_localization’s EKF and UKF use the same state and (effectively) the same process/measurement models; if the motion is only mildly nonlinear and your covariances are well-tuned, the EKF’s first-order linearization is already “good enough,” so the UKF has nothing extra to exploit.
Curvature not extreme enough: yaw rate and accelerations stayed in a range where the EKF linearization is accurate.
Strong constraints: wheel odom + gyro + (mag yaw) give frequent, consistent corrections → little room to diverge.
Additive, near-Gaussian noise: UKF shines with strong nonlinearities, non-additive noise, or missing/rare measurements. You don’t have those (yet).
Same tuning: identical Q/R implies nearly identical innovations and gains.
These are practical, controlled changes that usually produce a visible separation:
Intermittent absolute yaw
Disable mag-yaw for 3–5 s around each reversal; re-enable on straights. (E.g., make your mag-yaw node publish orientation_covariance[8]=-1 during “off” windows.)
Effect: forces reliance on the process model precisely where nonlinearity is largest.
Time offset (classic)
Add +30–50 ms latency
to either IMU or odom (not both). Even tiny misalignment shows up as
oscillatory XY error; the UKF typically copes a bit better.
Slip bursts at reversals
In the tracked controller, scale inner/outer track speeds for ~0.5 s at each sign change (e.g., inner ×0.8).
Effect: non-additive, state-dependent process noise → UKF advantage.
Vary steering efficiency over the run
Drift it 0.55→0.48→0.55 slowly. EKF with fixed linearization struggles to keep up; UKF handles mild parameter drift better.
Reduce update rate / increase dt
Drop filter frequency to 10–15 Hz during reversals only (or decimate
measurements). Larger dt + curvature pushes EKF linearization.
Nonlinear measurements
If you can, add one synthetic range–bearing beacon or a pose reset every
N seconds; UKF handles the trigonometry without Jacobians.
State augmentation
Add a slowly varying yaw-rate bias state in your custom UKF (process b˙z∼N(0,Qb)). Keep the EKF without that state. Then run the same trajectory.
In your mag-yaw publisher, toggle “off” windows:
Set orientation_covariance[8] = -1.0 for 3 s every 6 s (filter ignores yaw); restore to ~0.02 otherwise.
In your driver, keep the left↔right flips but add 20–40 ms command latency (you already have a queue—set latency_ms=30) and tighten segments (seg_min=0.8, seg_max=1.4, w_mag≈1.0, dwell=0.25).
In the Gazebo tracked plugin, at each reversal multiply the inner track velocity by 0.8 for 0.4 s.
EKF–UKF separation: plot ∥pEKF−pUKF∥ and ∣ψEKF−ψUKF∣. Spikes at reversals indicate different behavior.
Per-reversal RMSE: window ±20° around each sign change; report mean/95th.
NIS (innovation) time series: expect >1 near reversals, ~1 on straights.
Loop-closure drift per cycle (distance between successive S “nodes”): should increase when mag is off and drop when it comes back.
If your application will always have IMU+encoders, frequent updates, and modest curvature, then “EKF ≈ UKF” is an honest, useful finding: the simpler EKF is sufficient and cheaper to run. You can write that up clearly:
“Across circle and S-trajectory tests with IMU+encoders, UKF provided no statistically significant improvement over EKF (positional RMSE within X cm, yaw RMSE within Y deg). Therefore EKF suffices for arc-based navigation under these sensing conditions.”
Your current plot supports that conclusion. If you decide to pursue a separation, implement 1–2 of the bolded changes above—you’ll see divergence immediately.
Totally fair worries. Two things can make EKF≈UKF even on “stress” runs:
Your pipeline is effectively linear (the tracked controller + sim contact make turning very predictable).
Your EKF and UKF are configured so similarly that they ingest the same (well-behaved) data with the same covariances → same answer.
Here’s a tight checklist to rule out “something in the code” and to test the “controller is too linear” hypothesis.
Are the two filters really different?
Make sure EKF and UKF don’t subscribe to each other’s outputs or to the same fused topic by accident.
Confirm the measurement vectors differ only as intended: imu0_config, imu1_config, odom0_config identical? If yes, they’ll behave nearly identically unless dynamics are strongly nonlinear.
Time alignment
Plot cmd_vel.angular.z vs measured yaw rate from IMU/odom. You should see clear sign flips and close timing.
If they don’t flip, the issue is upstream (driver/controller). If they flip but the robot still circles one way, the plugin saturates/clamps.
Actuator saturation/clamping (common with tracks)
In many Gazebo tracked plugins, inner track saturates first; at higher v, negative angular.z may be “weaker” than positive.
Test at lower v and higher |w|: if alternation suddenly works, you were saturating.
Try a brief zero-command dwell (200–400 ms) at each flip to discharge integrators/friction.
Innovation/NIS looks too perfect
If NIS ~1 everywhere, your Q/R may
be tuned so tightly that the process never deviates much → EKF and UKF
converge to the same thing. Loosen Q on v, yaw_rate slightly and see if they separate at reversals.
In sim it can be. Common reasons:
High friction + no slip model ⇒ nearly holonomic differential drive behavior.
Steering efficiency fixed & symmetric ⇒ constant mapping w = k*(v_r - v_l) with little state dependence.
No latency/compliance in tracks/tension ⇒ dynamics are first-order and easy for EKF.
Reduce lateral friction / add slip compliance on the tracks’ contact surfaces (Gazebo <surface><friction>; lower μ, add anisotropy if available).
Vary steering_efficiency over time (e.g., 0.55 → 0.48 → 0.55) or make it radius-dependent.
Inject asymmetry at reversals: scale inner track speed ×0.8 for 0.3–0.5 s when the sign flips.
Add small command latency (20–40 ms) to IMU or odom, not both.
Throttle update rate to 10–15 Hz during reversals (larger Δt magnifies nonlinearity).
Turn mag-yaw off in short windows around reversals (set orientation_cov[8] = -1) so the filter relies on the process model where it’s hardest.
If after these changes EKF and UKF still overlap, your scenario truly is linear enough that EKF is sufficient (which is a valid, publishable result).
Flip audit (proves controller alternation):
bashros2 topic echo /model/mmtr/cmd_vel | grep angular
ros2 topic echo /imu/out | grep z: # gyro z
You should see +w, -w, +w… in both.
Latency toggle: set 30 ms delay in your S driver. Replot EKF–UKF difference ∥pEKF−pUKF∥. Separation during flips ⇒ timing was the linearizer’s weak point.
Mag yaw dropout: 3 s OFF every 6 s. If EKF and UKF finally diverge at reversals, the scenario was previously over-constrained.
Per-reversal RMSE: compute RMSE in ±20° around each flip vs. mid-segment. If they’re the same, your dynamics/terrain are too benign.
Identical process_noise_covariance and initial_estimate_covariance + highly frequent measurements.
two_d_mode with roll/pitch ignored and yaw fused at high confidence → linear SE(2) nearly exact.
Mag yaw fused continuously with low variance → wipes out gyro nonlinearity.
If you want, paste your current filter YAMLs for both EKF and UKF; I’ll mark the few lines to tweak (Q/R and sensor toggles) to create a meaningful separation—or to confirm you can safely stick with EKF.
Looking at that plot, your “S” trajectory is still very predictable — the EKF and UKF lines are essentially identical, meaning the filter math never got pushed into a regime where UKF’s nonlinear state propagation had an advantage.
That could be because:
The controller is producing a perfectly repeatable, symmetric S (low slip, constant turn rate, minimal actuator lag).
The process and measurement models are so clean that the EKF’s linearization error is negligible.
Measurement updates are frequent and dominate, so both filters keep snapping back to the same corrected state.
If you want this to truly be a stress test, you’ll need to:
Break the symmetry — vary turn rates and segment lengths randomly.
Force higher slip — reduce simulated lateral friction or vary it mid-run.
Reduce measurement dominance — increase measurement noise covariances so the process model has to “think” more.
Add dynamics complexity — short stops, sudden reversals, track-speed asymmetry.
Right now, this plot says: for this motion and sensor config, EKF is good enough.
If you want to prove that UKF is needed, you’ll have to make the trajectory and noise more chaotic so linearization error becomes visible.
Do you want me to modify your SPathDriver
so it produces an “irregular” oscillating path that’s more likely to
expose the difference? That would directly test whether the filters
diverge under realistic non-linearities.
Thanks for posting the YAMLs—this explains exactly why your EKF and UKF look the same.
Same state & same Q/R. Your process_noise_covariance and initial_estimate_covariance
are (nearly) identical. If the motion/measurements are modestly
nonlinear, EKF’s linearization is already accurate, so the UKF has
nothing extra to exploit.
Both fuse very strong yaw info all the time.
imu0_config enables roll, pitch, and yaw orientation (all three are true) and all angular rates.
You also fuse a mag yaw (imu1) continuously.
=> Heading is extremely well constrained; both filters get pulled to the same yaw, removing most nonlinearity leverage.
Planar robot but Z, Vz are enabled. odom0_config has true for z and vz
(and your process/initial covariances include them). robot_localization
will still run fine, but you’re feeding a planar system with
unnecessary states that are quiet/clean—again reducing scenarios where
models disagree.
High update rates. EKF at 100 Hz, UKF at 30 Hz, but the sensors update frequently; measurement domination makes both outputs nearly identical.
IMU delay handling only on UKF. imu0_nodelay: true is set on UKF but not EKF; however, since your data look well aligned, this isn’t creating a meaningful difference.
You can do A (cleanup) even if you keep B (stress) off.
Constrain the problem to SE(2) and stop fusing redundant signals:
EKF
yamltwo_d_mode: true # constrain to x,y,yaw; rl handles roll/pitch internally
imu0_config: [false,false,false,
false,false,true, # only yaw orientation
false,false,false,
true, true, true, # all angular rates (good for yaw rate)
false,false,false]
imu1_config: [false,false,false,
false,false,true, # mag yaw only
false,false,false,
false,false,false,
false,false,false]
odom0_config: [true,true,false, # x,y only; drop z
false,false,false,
true, true, false, # vx, vy; drop vz
false,false,false,
false,false,false]
UKF — make it the same as EKF for apples-to-apples:
yamlfrequency: 100.0 # match EKF
two_d_mode: true
imu0_nodelay: true # keep if you like, but matching rates is more important
# same imu/odom configs as EKF above
Also consider raising mag yaw variance a bit so it acts as a long-term bound, not a hard leash, e.g. in your mag node use
orientation_covariance[8] ≈ 0.02 … 0.05 (rad^2).
Pick two from this list; they’re the most effective:
Mag yaw outages around reversals
In your mag-yaw publisher, set orientation_covariance[8] = -1.0 for 2–3 s around each S flip (filter ignores that message), then restore to ~0.02.
→ Forces reliance on the process model where nonlinearity is largest.
Intentional slip burst (at each flip)
In the tracked controller (or an odom wrapper), scale inner-track speed to 0.8 for 0.3–0.5 s on each sign change.
→ State-dependent, non-additive process noise—UKF usually handles this better.
Small timing mismatch
Add +30–50 ms delay to IMU or odom (not both).
→ Linearization error grows at reversals; UKF tends to be slightly more robust.
Vary steering efficiency slowly (0.55→0.48→0.55) over the run.
→ Parameter drift creates mild model mismatch that UKF can absorb better.
Reduce filter update to 15–20 Hz only during flips
→ Larger Δt + curvature reversal stresses linearization.
Verify true sign flips end-to-end
ros2 topic echo /model/mmtr/cmd_vel (see angular.z +/− epochs)
Compare to measured yaw rate (/imu/out or odom twist). If commands flip but yaw rate doesn’t, you’re saturating the inner track or the plugin clamps one direction.
Add a brief zero dwell at each flip (200–400 ms) in your driver to de-saturate the tracks before sending the opposite sign.
Ensure one publisher to /model/mmtr/cmd_vel (no other controller overwriting you).
If you adopt the planar configs above and still see EKF ≈ UKF across circle and S tests (with no outages/slip injections), that’s a valid, defensible finding:
“With IMU + wheel odometry and continuous yaw reference, both EKF and UKF yield indistinguishable performance on curved and alternating-arc trajectories. The simpler EKF is therefore sufficient for arc-based navigation under these sensing conditions.”
If you’d like, paste your mag-yaw publisher covariances and I’ll give the exact on/off values and timing windows to implement (1) cleanly.
Great—this confirms your command stream is correct:
You’re publishing linear.x = 0.3 with angular.z = +0.9, then a short zero dwell, then angular.z = -0.9, etc.
So the sign flip is definitely being sent to /model/mmtr/cmd_vel.
If the robot still settles into mostly one-direction turning, the problem isn’t in your driver—it's in actuation/simulation response. Here’s how to pin it down and fix it.
Actuator saturation / steering asymmetry.
At v=0.3 m/s, the inner track may be saturating or the plugin’s steering mapping is asymmetric, so +0.9 turns strongly but -0.9 is weaker → net CCW drift.
Friction/ground model “memory”.
With skid steering, lateral scrub + high μ can resist reversals. If the
zero dwell is too short, the vehicle keeps its previous yaw momentum and
“refuses” to flip cleanly.
Another node overriding your commands intermittently (less common, but check).
Verify the response flips (not just the command):
bashros2 topic echo /imu/out | grep z:
ros2 topic echo /model/mmtr/odometry | grep angular
You should see yaw rate change sign in the measurements. If commands flip but yaw rate doesn’t → saturation/asymmetry.
Lower linear speed & lengthen dwell:
Try linear.x=0.15, angular.z=±1.0, dwell=0.4 s. If flips start working, you were saturating.
Ensure you’re the only publisher:
ros2 topic info /model/mmtr/cmd_vel → confirm 1 publisher.
Increase dwell: 0.2 s may be too short. Use 0.4–0.6 s between flips to de-scrub the tracks.
Reduce forward speed: v=0.15–0.2 m/s for flips; keep w=0.9–1.2 rad/s.
Pulse the reversal: for the first 0.2 s after the dwell, command angular.z at 1.2–1.5× magnitude to punch through static friction.
(If available) bump steering_efficiency slightly (e.g., 0.6–0.7) or make it symmetric; some Gazebo tracked plugins bias one direction.
Lower lateral friction on the track contacts (or enable anisotropic friction) so reversals don’t “stick.”
Short test: run 6 flips with v=0.15, w=±1.0, dwell=0.5. Plot measured yaw rate; you should see clean +/− plateaus.
Plot command vs. measured yaw rate and compute their signs over time:
Fraction of time with sign(cmd_w) != sign(meas_w) around flips → indicates saturation.
Mean ratio |meas_w| / |cmd_w| for + and − turns separately → asymmetry if they differ.
If both signs appear in the measured yaw rate after the tweaks, your S will look like an S, and then you can re-run the EKF/UKF comparison. If they still track identically, that’s good evidence the scenario is near-linear and EKF suffices for your sensing setup.
Looks like you grepped every line that contains z: across one or more topics, so three different “z’s” got interleaved:
z: 10.5… → that’s almost certainly pose.position.z from odom (height in the sim world), not a rate.
z: 0.97…0.99 → that’s orientation.z
of a quaternion (unitless), not a rate. A quaternion component ≈0.98
corresponds to a large yaw, but you can’t read it as rad/s.
z: ~±1.02 → that’s the yaw rate you care about (either imu.angular_velocity.z or odom.twist.twist.angular.z). You can see it flips from about −1.02 to +1.06, i.e., the robot is reversing rotation direction when commanded.
To avoid mixing fields, echo each source explicitly:
bash# the command you send
ros2 topic echo /model/mmtr/cmd_vel --field angular.z
# gyro yaw rate (rad/s)
ros2 topic echo /imu/out --field angular_velocity.z
# odometry yaw rate (rad/s)
ros2 topic echo /model/mmtr/odometry --field twist.twist.angular.z
# quaternion yaw (convert later, but if you must:)
ros2 topic echo /odometry/filtered --field pose.pose.orientation
What this tells us:
The measured yaw rate does change sign (≈−1 to +1 rad/s). So your reversal commands are reaching the robot and the sim is responding in both directions.
The big 10.5 values are unrelated (height); the 0.97…0.99 values are quaternion components.
If you want to quantify the actuation symmetry and timing, capture short logs and compute:
Sign agreement around each flip: sign(cmd_w) == sign(meas_w) percentage.
Gain asymmetry: mean |meas_w|/|cmd_w| for positive vs. negative turns.
Lag: cross-correlate cmd_w vs. meas_w to estimate delay (should be near zero; >20–30 ms will hurt).
If those look balanced, then the reason EKF≈UKF is simply that your scenario is still pretty linear/over-constrained—not a bug in your code.
Yeah—what you’re seeing (EKF≈UKF, super-clean S turns) is a hint the sim is too ideal for a skid-steer/tracked platform. Real tracked vehicles are messy: asymmetric slip, latency, compliance, temperature-dependent bias, and terrain anisotropy. If your simulator doesn’t include those, both filters will look the same and trajectories will look “perfect.”
Here’s a tight checklist to make realism (and stress) go up:
Symmetric, linear kinematics: fixed steering_efficiency and no state-dependent slip ⇒ w ≈ k·(v_r−v_l).
High, isotropic friction with no shear/compliance: tracks don’t scrub or plow.
No actuator limits or dynamics: instant response, no deadband, no saturation, no latency.
Over-clean sensors: gyro bias stable, mag perfectly aligned, encoders noiseless, no dropouts.
2.5D world but fused like 2D: flat contact, no micro pitch/roll that couples into yaw.
Command vs. measured yaw-rate: log cmd_vel.angular.z and imu.angular_velocity.z. Check:
delay (cross-correlation; expect 20–80 ms in real systems),
gain asymmetry (|meas|/|cmd| for + vs − turns),
reversal overshoot (peaks after sign flip).
Radius vs. command: drive constant ±w, measure turning radius; real tracked platforms give different radii for + vs − and drift with speed/load.
Slip signature: compare encoder-integrated distance vs. ground-truth path length; ratio should vary with curvature.
Contact & surface
Reduce/anisotropically set friction: <mu>, <mu2>; add <fdir1> (longitudinal) and <slip1/slip2>.
Lower <kp>, <kd> or increase <min_depth> to allow micro-sinkage (helps scrub).
Use heterogeneous terrain patches (μ, slip) along the path.
Tracks & chassis
Add compliance: soft joints/track tension model, small backlash.
Increase inertia & add off-center mass to couple pitch/roll into yaw on bumps.
Vary steering_efficiency with speed or periodically (e.g., 0.55→0.48→0.55).
Actuators & control
Add motor dynamics: rate limits, current limits, torque saturation, deadbands.
Inject latency (20–50 ms) and command quantization.
Asymmetric inner/outer track limits near reversals (realistic “stick–slip”).
Sensors
Gyro: bias instability + temperature drift (random walk), not just white noise.
Magnetometer: tilt errors + hard/soft-iron offsets; occasional dropout.
Encoders: quantization, missed ticks bursts, per-track scale mismatch (±1–3%).
Timestamp jitter (±3–5 ms).
Don’t fuse mag yaw continuously with tiny variance; raise it (e.g., 0.02–0.05 rad²) or gate it near reversals.
Allow time-varying R/Q (higher R during tight turns).
Optionally add a bias state in the UKF (yaw-rate bias) so it can differ from the EKF.
Lower lateral friction (μ2) and set nonzero <slip2>.
Add 30 ms IMU latency and 0.5-tick encoder quantization.
Increase mag yaw variance and disable it for 2–3 s around each S reversal.
Add motor rate limits + 0.3–0.5 s reversal dwell.
You should then see:
measured yaw-rate lag/overshoot at flips,
asymmetric turning radii,
growing EKF–UKF separation specifically during curvature reversals,
higher per-reversal RMSE and NIS spikes (then recovery on straights).
If after these adjustments EKF and UKF still match, that’s actually a strong result: for your sensing stack and expected dynamics, EKF is sufficient. But with the realism tweaks above, you’ll either validate that conclusion across harsher conditions—or uncover the differences you expected.
Yep — with mu and mu2 both at 5.0 for all flippers, you’ve basically got Velcro on a marble floor.
That will:
Kill lateral slip → turns are unrealistically perfect.
Make direction reversals crisp → no overshoot or lag.
Suppress scrubbing losses → both EKF and UKF see textbook kinematics.
In reality, for a tracked platform, μ2 (lateral) is far lower than μ (longitudinal).
For example, a real skid-steer might be something like:
xml<mu>1.0</mu> <!-- longitudinal (along track) -->
<mu2>0.2</mu2> <!-- lateral (sideways slip) -->
…and with slip parameters:
xml<slip1>0.05</slip1> <!-- longitudinal slip -->
<slip2>0.1</slip2> <!-- lateral slip -->
That lets the track scrub sideways during turns, giving:
Turning radius changes with load/surface.
Slight drift in yaw during straights.
More realistic controller “fight” during S-maneuvers.
A measurable difference between EKF and UKF performance.
Also, angular velocity_decay at 0.8
is heavy damping — fine for stability, but it’ll further “lock” parts
in place. If you’re aiming for realism, you might try 0.0–0.2 instead
and let friction/slip do the work.
If you want, I can suggest a minimal Gazebo config tweak to break the “perfect circle” behaviour without making the sim unstable. That would give you more slip and make your filters show differences.
Nice—this is headed in the right direction. A few important notes and quick wins:
You have:
xml<gazebo reference="base_link">
<mu1>5.0</mu1>
<mu2>5.0</mu2>
...
</gazebo>
But ODE friction must live under the collision element:
xml<gazebo reference="base_link">
<collision>
<surface>
<friction>
<ode>
<mu>...</mu>
<mu2>...</mu2>
<slip1>...</slip1>
<slip2>...</slip2>
</ode>
</friction>
</surface>
</collision>
</gazebo>
The top-level <mu1>/<mu2> tags won’t take effect on most Gazebo builds. Move friction into each contacting link’s collision (base and tracks/flippers).
To make turning more realistic (more scrub + variability), also add slip:
xml<ode>
<mu>1.0</mu> <!-- along the track -->
<mu2>0.15</mu2> <!-- lateral -->
<slip1>0.03</slip1>
<slip2>0.08</slip2> <!-- increases sideways “give” during turns -->
</ode>
Tweak: lower mu2 to 0.1–0.2 and raise slip2 to 0.08–0.15 if you want stronger, more realistic drift in S-curves.
velocity_decay.angular=0.2 on the flippers is fine, but the base_link has none shown (good). If you see “stuck” behavior on reversals, drop angular velocity decay to 0.0–0.05 and let friction/slip do the work.
The TrackedVehicle + TrackController setup looks consistent (same links on both sides).
To increase nonlinearity:
Reduce max_acceleration to 0.2–0.3 m/s² and add a small command latency (20–40 ms) in your driver.
Vary steering_efficiency a little over time (e.g., 0.55 → 0.48 → 0.55) or make it speed-dependent.
IMU noise/bias blocks are sensible. If you want more realism, add a random walk to gyro bias (slow drift).
You’ve included a LIDAR now; that’s fine if you’re not fusing it. Just mention in the paper it’s unused for odometry.
Your latest path still looks smooth and well-behaved. With μ/μ2 changes alone, the kinematics remain predictable, and you’re fusing continuous yaw (IMU + mag), so both filters will match closely. If you want separation:
Mag yaw dropout windows around each S flip (set orientation_cov[8] = -1 for 2–3 s, then restore to ~0.02–0.05).
Slip burst at reversals: for 0.3–0.5 s after each sign change, scale the inner track speed ×0.8 in a controller wrapper.
Timing mismatch: add +30–50 ms delay to IMU or odom.
Lower μ2 and nonzero slip2 as above.
xml<gazebo reference="front_flipper_left">
<self_collide>true</self_collide>
<velocity_decay><linear>0.0</linear><angular>0.05</angular></velocity_decay>
<collision>
<surface>
<friction>
<ode>
<mu>1.0</mu>
<mu2>0.15</mu2>
<slip1>0.03</slip1>
<slip2>0.10</slip2>
</ode>
</friction>
</surface>
</collision>
</gazebo>
Apply the same pattern to all
links that touch the ground (both flippers/tracks and base if it can
touch). Move any base_link friction from the top-level block into <collision>.
Bottom line: Fix the base_link friction placement, add slip2,
and optionally introduce small latency/mag-yaw dropouts. That will make
the S-trajectory visibly harder (more realistic) and is the fastest
path to reveal any EKF vs UKF differences.
This looks much better. You’ve moved friction into the collision/surface/friction/ode blocks and added anisotropy + slip—exactly what was missing. A few sharp suggestions to squeeze more realism and to make the S-test bite harder:
Anisotropic friction on all contacting links: mu=1.0, mu2=0.15.
Slip enabled: slip1=0.03, slip2=0.10. This will finally allow sideways scrub.
Reasonable angular velocity decay (0.2) on the track links (you can tune this).
Does base_link actually touch the ground?
If it never contacts, its friction settings won’t matter. That’s
fine—just ensure the four flipper/track links are the only ground
contacts (as you’ve configured).
Contact solver gains (kp/kd)
Your kp=10000, kd=100 are stiff; high stiffness can mask slip. If you see chatter/“sticky” contacts, try kp≈2000–5000, kd≈10–50.
Damping vs friction
With realistic friction/slip, you can usually lower velocity_decay.angular to 0.05–0.1. Let friction/slip drive the dynamics rather than artificial damping.
Units sanity
tracks_separation=0.2264 and tracks_height=0.6958
look inverted (separation usually wider than height). If those are
plugin-specific or deliberate, ignore; otherwise double-check.
Pick 2–3 of these. They reliably expose differences between EKF and UKF:
Reduce lateral grip a bit more:
On the tracks: mu2=0.10–0.12, slip2=0.12–0.15. This increases curvature-dependent drift.
Add actuator dynamics/limits:
In your driver, rate-limit cmd_vel.angular.z (e.g., 3 rad/s²) and add 30–50 ms latency. Real tracked vehicles don’t flip yaw rate instantly.
Vary steering efficiency slowly:
Change it over time via a small plugin or parameter update (e.g., 0.55 →
0.48 → 0.55 over 20–30 s). This creates mild model mismatch the UKF
handles better.
Mag yaw gating at reversals:
In your mag-yaw node, set orientation_cov[8] = -1 for ~2–3 s around each S flip (filter ignores mag) and restore to ~0.02–0.05 rad². Forces reliance on the process model when nonlinearity is highest.
Inner-track slip burst at flips:
For the first 0.3–0.5 s after a sign change, scale the inner track’s
commanded speed ×0.8. That produces realistic, asymmetric overshoot.
Command vs measured yaw rate: cmd_vel.angular.z vs imu.angular_velocity.z
– Show lag, overshoot, and gain asymmetry (+ vs − turns).
Per-sector SSE on the S: your polar error plot already; it should now show lobes at each reversal.
XY error & yaw error over time with vertical lines at flip times. Peaks should align with flips.
NEES/NIS (optional): innovation consistency spikes at flips show the model being challenged.
If you implement just (a) lower mu2 + higher slip2, (b) mag yaw gating at reversals, and (c) a tiny actuator delay/rate limit, you should see:
Larger error specifically during curvature reversals,
A clearer separation EKF vs UKF (UKF often recovers faster),
Still-good performance on straights and gentle arcs.
Shout if you want a tiny helper node for mag-yaw gating or a wrapper that injects latency/rate-limits into cmd_vel.
Nice—this is exactly what “more-realistic slip” looks like. A few quick takeaways from your numbers/plot:
Positional RMSE:
X ≈ 0.375 m (dominant)
Y ≈ 0.033–0.036 m (tiny)
Yaw RMSE: 0.54–0.65° (still very good)
EKF vs UKF: nearly identical; UKF a hair better in Y, EKF a hair better in yaw—so the scenario is still close to linear/observable with your sensing stack.
The plot shows the filtered paths sitting slightly “inside/behind” the GT along the direction of travel. That’s a classic along-track error (distance/scale or timing), not a lateral/yawing problem.
Scale factor / drag bias (encoders under-report, or track model shortens path with slip).
Test: compare total path lengths:
sodom=∑∥Δpodom∥ vs sgt.
If sodom/sgt<1 by ~5–7%, that’s your X error.
Fix: adjust encoder meters-per-tick (or the tracked plugin’s effective radius/steering_efficiency) until sodom≈sgt.
Time lag between sensors/GT and odom (20–100 ms lag shows up as along-track offset).
Test: sweep a small time shift Δt on odom (±150 ms) and recompute RMSE; if it drops sharply, you have latency.
Fix: time-align topics (use exact time stamps, or add a compensating delay in your driver).
Process-model mismatch during slip (constant slip slows forward progress more than your model expects).
Test: compute tangent/normal decomposition of error:
Tangent unit vector at GT: t^i=∥pi+1gt−pigt∥pi+1gt−pigt
Along-track error e∥=(piest−pigt)⋅t^i
Cross-track error e⊥=(piest−pigt)⋅n^i with n^ perpendicular to t^.
If RMSE(e∥)≫RMSE(e⊥), it’s a speed/scale/timing issue, not heading.
Calibrate a scale factor: regress xest on xgt (or path-lengths) to get k; set encoder scale or track parameters so k→1.
Check latency: do the Δt sweep; if RMSE drops, add that delay to the faster stream (or compensate in your fusion).
Slightly increase lateral slip (μ2 ↓ to 0.10–0.12, slip2 ↑ to 0.12–0.15) to further stress the filters—this often separates UKF from EKF on sharp oscillations.
Gate mag at reversals for ~2–3 s (set orientation_cov[8] = −1 in your mag-yaw message); this forces the filters to ride the IMU + odom during the nonlinear bits.
Report both positional RMSE and the tangent/normal split to show: “>90% of the error is along-track; cross-track and yaw remain small.”
Add path-length ratio sodom/sgt and the best Δt from the latency sweep; that cleanly distinguishes scale vs timing error.
Note that with realistic slip enabled, EKF/UKF still keep sub-degree yaw and low cross-track error, which supports using circles/arcs as traversal primitives. The remaining error is largely a distance scale/timing bias that can be calibrated.
If you want, I can sketch the tiny scripts to compute:
path-length ratio,
Δt sweep for minimum RMSE,
along-/cross-track RMSE.
Yep — and that’s actually a result. EKF and UKF will look almost identical when:
the motion/measurement model is close to linear (your S is smooth, no violent transients),
yaw is well observed (IMU + mag), so heading errors stay tiny,
odom dominates position with fairly Gaussian errors (mainly an along-track scale/lag),
both filters use the same state, the same Q/R, and no nasty non-additive noise.
In that regime the UKF’s sigma-point advantages don’t buy much over a well-tuned EKF, so they converge to essentially the same estimate—as your plots show.
If you want them to diverge (to prove the UKF’s value) you need to push into a more nonlinear / poorly observed regime or add states the EKF linearizes badly:
Reduce measurement dominance
Gate or down-weight mag yaw during reversals (2–3 s, orientation_cov[8] = -1), or increase mag variance (e.g., 0.02–0.05 rad²).
Inject harsher nonlinearity
Lower mu2 and raise slip2 a bit more (e.g., mu2≈0.10–0.12, slip2≈0.12–0.15), add brief inner-track slip bursts on turn sign flips, and rate-limit + delay cmd_vel by 30–50 ms.
Add bias and scale states (where UKF tends to help)
Gyro yaw-rate bias bg with random walk.
Per-track encoder scale factors (or a single odom scale), so the filter can learn your along-track error.
Optional fixed time-delay state (or do a Δt sweep offline).
Use a non-additive measurement
Fuse a bearing-only cue (e.g., periodic compass with heavy soft-iron offsets) or intermittent contact events; UKF handles this more gracefully.
Your numbers are already strong:
Yaw RMSE ≈ 0.5–0.65°,
Cross-track (Y) RMSE ≈ 3–4 cm,
Error is mainly along-track (X) from scale/latency/slip.
That supports the claim: either filter is accurate enough for circles and arcs with your sensing stack. The remaining error is bias-like (distance/lag), not random drift—so calibrate it.
Path-length ratio sodom/sgt to show the scale bias.
Best-Δt RMSE sweep to show timing effects.
Along-/cross-track RMSE split (tangent vs normal to GT) to prove it’s mostly along-track.
Innovation/NIS consistency (optional): both filters consistent ⇒ “EKF sufficient here”.
Bottom line: they’re nearly the same because your scenario is still effectively linear and well-observed. That’s fine if your goal is reliability. If you need to demonstrate UKF superiority, push harder on slip, bias, latency, and measurement gating—or let the filter learn bias/scale states.
Gotcha—if your goal is to make EKF and UKF diverge on the same data, you need to push the problem where linearization hurts (EKF) and sigma-points help (UKF). Here’s a focused, do-this list that reliably creates separation, plus what you should see.
Extend the filter state (UKF handles this better):
Gyro yaw-rate bias bg with random walk
Process noise: qbg∼10−6−10−5 (rad/s)2/s
Odom scale sv (meters-per-tick or forward speed scale)
qsv∼10−8−10−6 s−1 with prior sv≈1
Optional: lateral slip factor k⊥ (couples yaw rate to sideways velocity)
EKF tends to underperform when these states couple multiplicatively into the motion model; UKF copes better.
Right now yaw is well observed, keeping both filters nearly identical.
Around each S reversal (±2–3 s), set orientation_cov[8] = -1 on your mag-yaw topic (ignored by Robot_localization).
Or increase mag yaw variance 3–5× during high angular rates.
Expected: transient heading unobservability → model nonlinearity dominates → UKF recovers faster, EKF lags/overshoots.
Do any two of these:
Slip bursts at sign flips: for 0.3–0.5 s after each change in cmd_vel.angular.z, scale inner-track speed ×0.75–0.85 in a wrapper.
Lower lateral grip / raise slip: tracks mu2 = 0.10–0.12, slip2 = 0.12–0.15.
Actuator dynamics: limit yaw-rate slew to 2–3 rad/s² and add 30–60 ms delay to cmd_vel or IMU (not both).
Unequal track gains: left/right scale mismatch of 2–3%.
These create curvature-dependent and asymmetric errors—classic UKF territory.
Fuse a bearing-only cue intermittently (e.g., your mag-yaw node publishes only yaw modulo 2π with larger covariance) or contact events (collision flags) that update position along the normal only. UKF sigma-points propagate angle wrap + nonlinear update better.
| Knob | Set to | What to see |
|---|---|---|
| Mag yaw gating | off during ±2 s at flips | EKF yaw error spikes 1–2°, UKF ~0.7–1.2° |
| Slip burst | inner track ×0.8 for 0.4 s | EKF cross-track overshoot; UKF smaller lobe |
| Delay | IMU +40 ms | Along-track RMSE ↑, EKF phase lag > UKF |
| Odom scale state | enabled (Q small) | UKF learns bias in 10–20 s; EKF slower/biased |
| Lateral slip raise | slip2=0.14 | Sector SSE peaks at reversals; UKF peak lower |
Mag-yaw gating (inside your mag node):
python# during high |yaw_rate| or scheduled flip windows:
out.orientation_covariance[8] = -1.0 # gate off yaw for RL
# otherwise:
out.orientation_covariance[8] = 0.02 # nominal (tune)
Cmd_vel wrapper (publish to real /cmd_vel):
python# on sign change of angular.z:
inner_scale = 0.8
burst_t = 0.4 # s
# for tracked vehicles, scale the inner side speed in the tracked controller
State extension (concept)
If you’re using robot_localization, you can’t add custom states without forking; instead, emulate via inputs:
publish /imu with a slowly varying bias (random walk) and see UKF vs EKF reaction,
publish odom with a small multiplicative scale drift, see which filter tracks it better.
If you can modify your UKF node: add states [bg,sv] and apply ψ˙=(ωz−bg), vxtrue=sv⋅vxodom.
Compute and report:
Along- vs cross-track RMSE (tangent/normal decomposition).
Sector SSE polar plot (you already have it): reversals should dominate; UKF bars shorter.
Best Δt sweep (apply time shift to odom/IMU and re-compute RMSE): EKF typically needs larger Δt to minimize error under delay.
Peak yaw error at each reversal and settling time back to |error|<0.5°. UKF should settle faster.
Optional NIS/innovation variance: larger spikes for EKF at flips.
If you apply mag gating + slip bursts + a small IMU delay, you should get clearly different curves: EKF will show bigger overshoot in heading and cross-track during each S flip and slower re-convergence; UKF will be visibly tighter. That’s the plot pair and table you want in your paper to justify the UKF.
Great—here’s a concrete, drop-in way to force EKF vs UKF to diverge, using only ROS 2 nodes you can run alongside your current setup.
Reduce/disable magnetometer yaw just when the motion is most nonlinear. This creates short periods where heading is weakly observed, so the process model dominates (UKF usually recovers faster).
What it does
Watches |imu.angular_velocity.z| and your commanded cmd_vel.angular.z.
When a sign flip or high yaw rate is detected, it publishes the same yaw as your current mag node but sets orientation_cov[8] = -1 (Robot Localization ignores it). Otherwise it uses your nominal variance (e.g., 0.02 rad²).
Node (Python, ROS 2): save as mag_yaw_gated.py
python#!/usr/bin/env python3
import math, rclpy, time
from rclpy.node import Node
from geometry_msgs.msg import Twist
from sensor_msgs.msg import Imu
from std_msgs.msg import Header
class MagYawGater(Node):
def __init__(self):
super().__init__('mag_yaw_gater')
# topics
self.sub_mag = self.create_subscription(Imu, '/imu/mag_yaw_raw', self.mag_cb, 20)
self.sub_imu = self.create_subscription(Imu, '/imu/cov_fixed', self.imu_cb, 50)
self.sub_cmd = self.create_subscription(Twist,'/model/mmtr/cmd_vel', self.cmd_cb, 20)
self.pub = self.create_publisher(Imu, '/imu/mag_yaw', 10)
# params
self.yaw_var_nom = float(self.declare_parameter('yaw_var_nom', 0.02).value) # rad^2
self.gate_secs = float(self.declare_parameter('gate_secs', 2.0).value) # seconds to gate after a flip
self.flip_thresh = float(self.declare_parameter('flip_thresh', 0.2).value) # sign flip if |Δωz|>this
self.rate_gate = float(self.declare_parameter('rate_gate', 0.8).value) # gate if |ωz|>this (rad/s)
self.rp_cov_big = 99999.0
# state
self.last_cmd_om = 0.0
self.last_flip_t = -1e9
self.last_imu_om = 0.0
self.latest_mag = None
self.get_logger().info('Mag yaw gater running.')
def cmd_cb(self, msg: Twist):
om = msg.angular.z
# detect sign flip
if (self.last_cmd_om == 0.0 and om != 0.0) or (om*self.last_cmd_om < 0.0 and abs(om - self.last_cmd_om) > self.flip_thresh):
self.last_flip_t = self.get_clock().now().seconds_nanoseconds()[0] + \
self.get_clock().now().seconds_nanoseconds()[1]*1e-9
self.last_cmd_om = om
def imu_cb(self, msg: Imu):
self.last_imu_om = msg.angular_velocity.z
def mag_cb(self, msg: Imu):
self.latest_mag = msg
now = self.get_clock().now().seconds_nanoseconds()
t = now[0] + now[1]*1e-9
gated = (t - self.last_flip_t) < self.gate_secs or abs(self.last_imu_om) > self.rate_gate
out = Imu()
out.header = Header()
out.header.stamp = self.get_clock().now().to_msg()
out.header.frame_id = msg.header.frame_id
out.orientation = msg.orientation
# ignore roll/pitch from this msg; provide yaw variance
if gated:
cov = -1.0 # RL ignores this orientation entirely
else:
cov = self.yaw_var_nom
out.orientation_covariance = [self.rp_cov_big,0,0, 0,self.rp_cov_big,0, 0,0,cov]
out.angular_velocity_covariance = [-1.0]*9
out.linear_acceleration_covariance = [-1.0]*9
self.pub.publish(out)
def main(args=None):
rclpy.init(args=args); n=MagYawGater(); rclpy.spin(n); n.destroy_node(); rclpy.shutdown()
if __name__=='__main__': main()
How to wire it
Rename your current mag-yaw output to /imu/mag_yaw_raw.
Run this node; Robot Localization should subscribe to /imu/mag_yaw (gated).
Wrap the velocity commands so the vehicle behaves more like a real tracked platform.
What it does
Adds a configurable latency to cmd_vel.
Rate-limits yaw rate (angular.z) slew.
On each sign flip of yaw, applies a short slip burst: scales linear speed down (forward traction loss) and optionally reduces effective yaw during the first 0.3–0.5 s.
Node (Python, ROS 2): save as cmd_vel_wrapper.py
python#!/usr/bin/env python3
import rclpy, collections, time
from rclpy.node import Node
from geometry_msgs.msg import Twist
class CmdVelWrapper(Node):
def __init__(self):
super().__init__('cmd_vel_wrapper')
self.sub = self.create_subscription(Twist, '/model/mmtr/cmd_vel_raw', self.cb, 50)
self.pub = self.create_publisher(Twist, '/model/mmtr/cmd_vel', 10)
# params
self.delay_s = float(self.declare_parameter('delay_s', 0.04).value) # 40 ms
self.max_slew = float(self.declare_parameter('yaw_slew', 3.0).value) # rad/s^2
self.slip_time = float(self.declare_parameter('slip_time', 0.4).value) # s
self.lin_scale = float(self.declare_parameter('slip_lin_scale', 0.8).value)
self.yaw_scale = float(self.declare_parameter('slip_yaw_scale', 0.9).value)
self.buf = collections.deque() # (t_arrive, Twist)
self.timer = self.create_timer(0.02, self.tick) # 50 Hz output
self.last_pub = Twist()
self.last_pub_time = None
self.last_cmd_sign = 0
self.flip_start_t = -1e9
def cb(self, msg: Twist):
t = self.get_clock().now().seconds_nanoseconds()
now = t[0] + t[1]*1e-9
self.buf.append((now + self.delay_s, msg))
def tick(self):
t = self.get_clock().now().seconds_nanoseconds()
now = t[0] + t[1]*1e-9
# dequeue ready command
cmd = None
while self.buf and self.buf[0][0] <= now:
_, cmd = self.buf.popleft()
if cmd is None:
cmd = self.last_pub # hold last
# detect sign flip
sign = 0 if abs(cmd.angular.z) < 1e-6 else (1 if cmd.angular.z > 0 else -1)
if sign != 0 and sign != self.last_cmd_sign:
self.flip_start_t = now
self.last_cmd_sign = sign
# apply slip burst for first slip_time seconds after flip
if (now - self.flip_start_t) < self.slip_time:
cmd_lin = cmd.linear.x * self.lin_scale
cmd_yaw = cmd.angular.z * self.yaw_scale
else:
cmd_lin = cmd.linear.x
cmd_yaw = cmd.angular.z
# yaw slew-rate limit
if self.last_pub_time is None:
dt = 0.02
else:
dt = max(1e-3, now - self.last_pub_time)
max_step = self.max_slew * dt
yaw_prev = self.last_pub.angular.z
yaw_cmd = max(min(cmd_yaw, yaw_prev + max_step), yaw_prev - max_step)
out = Twist()
out.linear.x = cmd_lin
out.angular.z = yaw_cmd
self.pub.publish(out)
self.last_pub = out
self.last_pub_time = now
def main(args=None):
rclpy.init(args=args); n=CmdVelWrapper(); rclpy.spin(n); n.destroy_node(); rclpy.shutdown()
if __name__=='__main__': main()
How to wire it
Make your driver publish to /model/mmtr/cmd_vel_raw (same messages as before).
Launch this wrapper; it republishes to the real /model/mmtr/cmd_vel.
Add these in your evaluation script (pseudocode you can paste into your existing analyzer):
Path-length ratio (scale bias)
pythons_gt = np.sum(np.linalg.norm(np.diff(np.c_[x_gt,y_gt], axis=0), axis=1))
s_ekf = np.sum(np.linalg.norm(np.diff(np.c_[x_ekf,y_ekf], axis=0), axis=1))
s_ukf = np.sum(np.linalg.norm(np.diff(np.c_[x_ukf,y_ukf], axis=0), axis=1))
print("path ratio EKF:", s_ekf/s_gt, " UKF:", s_ukf/s_gt)
Best Δt sweep (latency)
pythondef time_shift_rmse(times, x_gt, y_gt, times_est, x_est, y_est, shifts=np.linspace(-0.2,0.2,81)):
best = (1e9, 0)
for dt in shifts:
t_shifted = times_est + dt
# interpolate est to gt times
x_i = np.interp(times, t_shifted, x_est)
y_i = np.interp(times, t_shifted, y_est)
rmse = np.sqrt(np.mean((x_gt-x_i)**2 + (y_gt-y_i)**2))
if rmse < best[0]: best = (rmse, dt)
return best
rmse_min, dt_best = time_shift_rmse(t_gt, x_gt, y_gt, t_ekf, x_ekf, y_ekf)
print("EKF best Δt:", dt_best, " RMSE:", rmse_min)
Along-/cross-track error
pythongt = np.c_[x_gt,y_gt]; est = np.c_[x_ekf,y_ekf]
tgt = np.diff(gt, axis=0); tang = tgt / np.linalg.norm(tgt, axis=1, keepdims=True)
# pad last vector
tang = np.vstack([tang, tang[-1]])
err = est - gt
e_par = np.sum(err * tang, axis=1)
# normal is tang rotated 90°
norm = np.c_[-tang[:,1], tang[:,0]]
e_perp = np.sum(err * norm, axis=1)
print("EKF RMSE along:", np.sqrt(np.mean(e_par**2)), " cross:", np.sqrt(np.mean(e_perp**2)))
During each S flip (where mag yaw is gated and slip burst happens): EKF will show larger peak yaw error and cross-track overshoot; UKF should settle faster and smaller.
Between flips: both remain similar (well-observed, near-linear segments).
Metrics to report: peak yaw error per flip, settling time to |yaw error|<0.5°, along-/cross-track RMSE, sector SSE lobes at flip angles, and (optionally) best-Δt and path-length ratio.
Tune just three knobs to adjust “how much divergence”:
gate_secs (1.5–3.0 s),
slip_lin_scale (0.7–0.9) and slip_time (0.3–0.6 s),
delay_s (0.03–0.06 s) and yaw_slew (2–3 rad/s²).
If you want, I can also give you a tiny UKF patch with a yaw-rate bias state and an odom scale state—those two states alone typically make the UKF shine on these S-turns.
Yes — in my example, mag_yaw_raw is just a placeholder name for your existing /imu/mag_yaw topic.
The idea is:
Your current magnetometer yaw publisher → rename (or remap) its output to /imu/mag_yaw_raw in your launch file.
The gating node subscribes to /imu/mag_yaw_raw.
The gating node republishes on /imu/mag_yaw — with covariance changed depending on motion state.
Robot Localization (EKF/UKF) continues to subscribe to /imu/mag_yaw as before.
That way, you can intercept the mag data, modify it (temporarily “ignore” it during aggressive turns), and send it on without changing the rest of your system.
If you don’t want to rename the original publisher, you can instead:
Change the gating node to subscribe directly to /imu/mag_yaw and publish to /imu/mag_yaw_gated,
Then point your EKF/UKF params to the gated topic.
Do you want me to adjust the code so it works directly with your current /imu/mag_yaw without changing your launch?
Perfect—your launch file is already 80% wired for the “gate mag yaw” and “wrap cmd_vel” plan. Here’s exactly what to add/change.
Gazebo bridge remaps real command to a raw topic ✔️
swift/model/mmtr/cmd_vel → /model/mmtr/cmd_vel_raw
Your EKF/UKF configs still subscribe to /imu/mag_yaw ✔️ (keep that).
Your current nodes:
mag_yaw_pub publishes mag yaw. You’ve remapped its output to /imu/mag_yaw_raw ✔️
covariance_override.py remaps /imu/cov_fixed → /imu/mag_yaw_raw ❌ (this is odd; see note below)
We’ll insert two new small nodes:
mag_yaw_gated.py: subscribes /imu/mag_yaw_raw → publishes /imu/mag_yaw with covariance gated during reversals.
cmd_vel_wrapper.py: subscribes /model/mmtr/cmd_vel_raw → publishes the actual /model/mmtr/cmd_vel with delay/slew/slip-burst.
mag_yaw_gated.py (put in your mmtr_description/scripts or nodes)Gates the magnetometer yaw for a few seconds after each left↔right flip, or whenever yaw rate is high.
python#!/usr/bin/env python3
import math
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from sensor_msgs.msg import Imu
from std_msgs.msg import Header
class MagYawGater(Node):
def __init__(self):
super().__init__('mag_yaw_gater')
# inputs
self.sub_mag = self.create_subscription(Imu, '/imu/mag_yaw_raw', self.mag_cb, 20)
self.sub_imu = self.create_subscription(Imu, '/imu/cov_fixed', self.imu_cb, 50)
self.sub_cmd = self.create_subscription(Twist, '/model/mmtr/cmd_vel_raw', self.cmd_cb, 20)
# output
self.pub = self.create_publisher(Imu, '/imu/mag_yaw', 10)
# params
self.yaw_var_nom = float(self.declare_parameter('yaw_var_nom', 0.02).value) # rad^2 when NOT gated
self.gate_secs = float(self.declare_parameter('gate_secs', 2.0).value) # seconds after a flip
self.rate_gate = float(self.declare_parameter('rate_gate', 0.8).value) # |ωz| above this gates
self.flip_thresh = float(self.declare_parameter('flip_thresh', 0.2).value) # min Δ at flip
self.rp_cov_big = 99999.0
# state
self.last_cmd_om = 0.0
self.last_flip_t = -1e9
self.last_imu_om = 0.0
self.get_logger().info('Mag yaw gater running.')
def cmd_cb(self, msg: Twist):
om = msg.angular.z
# detect sign flip (± to ∓ with a meaningful jump)
if (om * self.last_cmd_om < 0.0) and (abs(om - self.last_cmd_om) > self.flip_thresh):
self.last_flip_t = self.get_clock().now().nanoseconds * 1e-9
self.last_cmd_om = om
def imu_cb(self, msg: Imu):
self.last_imu_om = msg.angular_velocity.z
def mag_cb(self, msg: Imu):
now = self.get_clock().now().nanoseconds * 1e-9
gated = ((now - self.last_flip_t) < self.gate_secs) or (abs(self.last_imu_om) > self.rate_gate)
out = Imu()
out.header = Header()
out.header.stamp = self.get_clock().now().to_msg()
out.header.frame_id = msg.header.frame_id
out.orientation = msg.orientation
# Tell RL: ignore roll/pitch here, and either ignore yaw entirely (-1) or use nominal variance
out.orientation_covariance = [
self.rp_cov_big, 0.0, 0.0,
0.0, self.rp_cov_big, 0.0,
0.0, 0.0, (-1.0 if gated else self.yaw_var_nom)
]
out.angular_velocity_covariance = [-1.0]*9
out.linear_acceleration_covariance = [-1.0]*9
self.pub.publish(out)
def main():
rclpy.init(); rclpy.spin(MagYawGater()); rclpy.shutdown()
if __name__ == '__main__':
main()
cmd_vel_wrapper.pyAdds ~40 ms delay, yaw-rate slew limit, and a short slip burst at sign flips.
python#!/usr/bin/env python3
import collections
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
class CmdVelWrapper(Node):
def __init__(self):
super().__init__('cmd_vel_wrapper')
self.sub = self.create_subscription(Twist, '/model/mmtr/cmd_vel_raw', self.cb, 100)
self.pub = self.create_publisher(Twist, '/model/mmtr/cmd_vel', 10)
# params
self.delay_s = float(self.declare_parameter('delay_s', 0.04).value) # 40 ms
self.max_slew = float(self.declare_parameter('yaw_slew', 3.0).value) # rad/s^2
self.slip_time = float(self.declare_parameter('slip_time', 0.4).value) # s after flip
self.lin_scale = float(self.declare_parameter('slip_lin_scale', 0.8).value)
self.yaw_scale = float(self.declare_parameter('slip_yaw_scale', 0.9).value)
self.buf = collections.deque() # (release_time, Twist)
self.timer = self.create_timer(0.02, self.tick) # 50 Hz
self.last_pub = Twist()
self.last_t = None
self.last_sign = 0
self.flip_t = -1e9
def now_s(self):
return self.get_clock().now().nanoseconds * 1e-9
def cb(self, msg: Twist):
self.buf.append((self.now_s() + self.delay_s, msg))
def tick(self):
now = self.now_s()
# pop ready commands
cmd = None
while self.buf and self.buf[0][0] <= now:
_, cmd = self.buf.popleft()
if cmd is None:
cmd = self.last_pub # hold last
sign = 0 if abs(cmd.angular.z) < 1e-6 else (1 if cmd.angular.z > 0 else -1)
if sign != 0 and sign != self.last_sign:
self.flip_t = now
self.last_sign = sign
# slip burst window
lin = cmd.linear.x
yaw = cmd.angular.z
if (now - self.flip_t) < self.slip_time:
lin *= self.lin_scale
yaw *= self.yaw_scale
# slew limit yaw
dt = 0.02 if self.last_t is None else max(1e-3, now - self.last_t)
max_step = self.max_slew * dt
yaw_prev = self.last_pub.angular.z
yaw = max(min(yaw, yaw_prev + max_step), yaw_prev - max_step)
out = Twist()
out.linear.x = lin
out.angular.z = yaw
self.pub.publish(out)
self.last_pub = out
self.last_t = now
def main():
rclpy.init(); rclpy.spin(CmdVelWrapper()); rclpy.shutdown()
if __name__ == '__main__':
main()
Make both files executable:
bashchmod +x mag_yaw_gated.py cmd_vel_wrapper.py
Add them to your package (setup.py/entry_points or CMake) as console scripts.
Add these two Nodes to your LaunchDescription (after the bridge):
pythonmag_yaw_gater = Node(
package="mmtr_description",
executable="mag_yaw_gated.py",
parameters=[{'yaw_var_nom': 0.02, 'gate_secs': 2.0, 'rate_gate': 0.8}],
# no remaps: sub /imu/mag_yaw_raw, pub /imu/mag_yaw
)
cmd_vel_wrapper = Node(
package="mmtr_description",
executable="cmd_vel_wrapper.py",
parameters=[{'delay_s': 0.04, 'yaw_slew': 3.0, 'slip_time': 0.4,
'slip_lin_scale': 0.8, 'slip_yaw_scale': 0.9}],
# no remaps: sub /model/mmtr/cmd_vel_raw, pub /model/mmtr/cmd_vel
)
And then add:
pythonld.add_action(cmd_vel_wrapper) ld.add_action(mag_yaw_gater)
Right now you have:
pythoncov_overide = Node(
package="mmtr_description",
executable="covariance_override.py",
remappings=[('/imu/cov_fixed', '/imu/mag_yaw_raw')]
)
This remap would make anyone subscribing to /imu/cov_fixed receive the mag yaw (!) which is unintended.
Remove that remap (or drop this node entirely if you don’t need it). Keep your IMU at /imu/cov_fixed. Your mag_yaw_pub should publish to /imu/mag_yaw_raw only.
So:
pythoncov_overide = Node(
package="mmtr_description",
executable="covariance_override.py",
# remove the remappings; or comment this node out if not needed
)
Your existing mag_yaw_pub remap is good:
pythonmag_yaw_pub = Node(
package="mmtr_description",
executable="mag_yaw_pub.py",
remappings=[('/imu/mag_yaw', '/imu/mag_yaw_raw')]
)
Now the flow is:
bashmag_yaw_pub → /imu/mag_yaw_raw → (mag_yaw_gated) → /imu/mag_yaw → EKF/UKF driver/bridge → /model/mmtr/cmd_vel_raw → (cmd_vel_wrapper) → /model/mmtr/cmd_vel → Gazebo
Start with these defaults; then increase the effect if the curves still overlap:
gate_secs: 2.0–3.0 s (length of mag blackout after each flip)
rate_gate: 0.8 rad/s (gate when spinning fast)
delay_s: 0.04–0.06 s
yaw_slew: 2.0–3.0 rad/s² (smaller = more lag)
slip_time: 0.3–0.6 s
slip_lin_scale: 0.8 (reduce forward speed right after flips)
slip_yaw_scale: 0.9
If you still get EKF≈UKF:
Lower your track mu2 to 0.10–0.12 and raise slip2 to 0.12–0.15,
Increase the mag yaw nominal variance to 0.03–0.05 rad².
ros2 topic echo /model/mmtr/cmd_vel_raw --field angular.z vs /model/mmtr/cmd_vel --field angular.z
→ you should see the delay + slew limiting and the reduced magnitude right after flips.
ros2 topic echo /imu/mag_yaw
→ during gated windows, orientation_covariance[8] should be −1.0.
EKF/UKF params: keep subscribing to /imu/mag_yaw (gated) and do not fuse roll/pitch from mag (your configs already do that).
Run the S-trajectory again and plot:
peak yaw error per flip and settling time,
cross-track error spikes at flips,
sector SSE bars (reversals should dominate),
EKF vs UKF separation (UKF typically smaller peaks & faster recovery).
If you want me to fold these nodes into your package’s setup.py entry points or to return minimal diffs against your launch, say the word and I’ll draft them.
Yes — that’s exactly what you’d want if you’re inserting a cmd_vel_wrapper.py between your teleop/autonomous commands and Gazebo.
Right now:
Your FlipS node publishes directly to /model/mmtr/cmd_vel → Gazebo gets it raw.
After change:
FlipS publishes to /model/mmtr/cmd_vel_raw.
cmd_vel_wrapper.py subscribes to …/cmd_vel_raw and outputs the processed/delayed/slipped commands to /model/mmtr/cmd_vel.
Gazebo only ever sees the wrapped commands.
This way every source of velocity commands—whether teleop, waypoint follower, or scripted test—gets the same delay/slew/slip realism without modifying them individually.
A few extra points to make sure this change actually works as intended and gives you the “stress test” conditions you want:
1. Make sure your remapping matches everywhere
In your launch file, you’ve already got:
pythonremappings=[
("/model/mmtr/cmd_vel", "/model/mmtr/cmd_vel_raw"),
]
But that’s in the bridge, so check that any other nodes (like teleop) don’t bypass this.
Your cmd_vel_wrapper.py should listen to /model/mmtr/cmd_vel_raw and publish to /model/mmtr/cmd_vel with your injected effects.
2. Inject disturbances in the wrapper
If your goal is to make EKF and UKF outputs diverge more noticeably, the wrapper is the perfect place to add:
Latency (e.g., time.sleep(0.1) before publishing).
Small random noise on linear.x and angular.z.
Occasional dropouts (publish nothing for a few cycles).
Small overshoot on angular.z to mimic oversteering in tracked vehicles.
This will push the state estimators harder without having to rebuild the Gazebo physics.
3. Avoid “perfect symmetry”
Tracked vehicles in simulation often turn too cleanly because both track controllers get identical inputs at identical times.
You can introduce tiny differences between left/right track velocities in the wrapper so that curves aren’t perfectly smooth.
This will especially stress yaw estimation and might produce that divergence you’re looking for.
4. Use faster direction changes
If you want the S-path to feel like a stress test, shorten your segment times and increase angular velocity slightly.
Right now your tests might be too slow and gentle, which lets the filters behave similarly.
Faster changes force the UKF’s nonlinear advantage to show more.
5. Record mag_yaw_raw
Yes, your /imu/mag_yaw remapped to /imu/mag_yaw_raw is what you think—it’s the raw heading from the magnetometer.
If you include that in your plots alongside odometry and filtered yaw, you’ll see how well each filter tracks heading drift and bias.
If you want, I can give you a short cmd_vel_wrapper.py that adds latency, noise, and left/right track mismatch so you can instantly make your “S” more of a stress test.
Would you like me to write that?
This plot still shows EKF and UKF tracking almost identically, which means your simulation is still too “clean” — both filters are basically seeing the same low-noise, well-synchronized data.
If you really want the curves to separate:
1. Increase IMU bias/noise dynamically
Right now, your IMU noise and bias are fixed in the URDF. You can:
Increase bias_mean for angular velocity z (e.g., 0.05–0.1) to simulate a strong yaw drift.
Or vary it during the run with a plugin to simulate thermal drift.
2. Add wheel slip & steering inefficiency
In your <TrackedVehicle> plugin:
Reduce steering_efficiency below 0.5 (even 0.3–0.4) to cause understeer in curves.
Increase <slip1> and <slip2> in the friction tags for all tracks. This will create path deviation and make the odometry diverge from ground truth.
3. Inject actuator mismatch
In your cmd_vel wrapper:
Slightly scale left and right velocities differently (+5% / -5%).
Add a small Gaussian noise to angular.z.
4. Make magnetometer yaw noisy
If your /imu/mag_yaw_raw is perfect, both filters will correct yaw too easily.
Add synthetic noise or periodic bias to that topic in your mag_yaw_pub.py.
5. Use aggressive maneuvers
Drive patterns with sharp S-turns, quick reversals, and varying speeds.
Slow, steady motion doesn’t challenge the estimators.
If you want, I can give you a ready-to-drop-in Python wrapper that intercepts /cmd_vel_raw, adds slip, noise, and bias, and republishes to /cmd_vel so you’ll definitely see EKF and UKF separate in plots.
Do you want me to make that?
Short answer: you’re not crazy—what you’re seeing is very plausible.
When the vehicle + simulator behave
“too linear,” an EKF and a UKF will often look nearly identical. The
UKF’s advantage only shows up when the state evolution or measurements are strongly nonlinear, biased, delayed, or poorly modeled. If the TrackedVehicle controller is effectively converting cmd_vel
into an almost-ideal body twist with mild first-order limits, your
motion model is close to linear and both filters collapse to the same
answer.
Controller linearization: If the plugin maps cmd_vel → base_link twist
directly (vs. simulating per-track dynamics and ground interaction),
you get near-ideal kinematics. Small steering inefficiency alone doesn’t
make it very nonlinear.
High-rate, clean measurements: IMU yaw + magnetometer yaw dominates heading; wheel odom is smooth; covariances are consistent → both filters are measurement-driven.
Same models/tuning: EKF & UKF using the same process and measurement models + similar Q/R ⇒ same outcome.
No strong biases/delays: Constant biases (gyro, wheel scale) or time skew between topics are where UKF sometimes pulls ahead; you likely don’t have enough of those yet.
(quick checks you can compute from the data you already log)
Innovation (residual) stats for each measurement: mean ≈ 0, variance ≈ R → system is well modeled & linear enough.
NEES/NIS consistency: near the expected χ² bounds → filter is linear-enough & well tuned.
Mag yaw gating: Temporarily disable /imu/mag_yaw for segments. If EKF & UKF still match closely, the motion is being linearized by the controller+terrain.
Pick 2–3 of these; you don’t need them all.
Reduce measurement authority
Run “IMU-only yaw” intervals: disable mag yaw for 5–10 s windows (or downsample it to 1–2 Hz). Gyro drift + slip will accumulate and stress the prediction.
Inflate odom pose R slightly (e.g., x/y to 0.1–0.2 m²) so the filter relies more on the motion model between updates.
Introduce model nonlinearity instead of more slip
Control left/right track velocities directly (two commands) with separate rate limits and saturation; avoid a body-twist controller. Even a ±3–5% asymmetry and different accel limits per track adds real nonlinearity.
Add a speed-dependent steering efficiency (e.g., eff = k0 + k1*v²) rather than a constant 0.5.
Temporal effects (classic EKF pain points)
Inject timestamp jitter or a fixed sensor delay (e.g., 50–100 ms) on one sensor (odom or IMU). UKF often tolerates this a bit better.
Lower the UKF/EKF update frequency so more propagation happens between corrections (e.g., 20–30 Hz), then excite with quicker turns.
Biases & scale factors (make them states)
Add gyro z-bias and wheel scale factor as states in both filters. Then:
– Slowly drift the true bias in sim;
– Vary the wheel scale ±1–2% between left/right.
The filter that handles the parameter nonlinearity better will separate.
Excite real nonlinearity in the path
Use sharp S-curves with abrupt sign changes in ω at nonuniform intervals and bursty speed (stop-go). Smooth sinusoids are still pretty linear.
Prefer per-track control (two controllers) over body twist.
Add rate limits and different dynamics L vs R (accel, jerk).
Confirm that ground contact actually affects twist: try a low-μ patch or a ramp. If base_link twist remains almost ideal, the plugin is linearizing.
A/B runs: with & without mag yaw (or at 1 Hz).
Per-track control with ±5% asymmetry + rate limits.
Inject 80 ms delay on odom messages only.
Re-run your S-stress path with faster direction changes.
Then compare:
EKF vs UKF pos/yaw RMSE,
Innovation RMS time series,
XY error over time (you already plot this—good),
Sector error plots for where it diverges.
If after #1–#4 they still overlap, that’s actually a useful result: in this operating envelope, the simpler EKF is sufficient—and you can say so confidently, backed by diagnostics.
Want a small cmd_vel_wrapper.py
that implements per-track asymmetry, delay, jitter, and occasional
dropouts so you can flip stress on/off with params? I can paste a
ready-to-run node.
Overall your script is solid: you align by timestamp with merge_asof,
compute XY/yaw errors correctly (with unwrap + wrap-to-π), and plot the
right comparisons. A few small gotchas and easy upgrades will make it
tighter and more diagnostic.
Yaw extraction convention
R.from_quat([x,y,z,w]).as_euler("xyz")[2] is OK if you intend intrinsic XYZ (roll-pitch-yaw). To avoid any ambiguity, make it explicit you want roll-pitch-yaw:
pythondef quaternion_to_yaw(q_list):
# roll, pitch, yaw in radians
return R.from_quat(q_list).as_euler("xyz", degrees=False)[2]
If the robot can pitch/roll noticeably, this still matches ROS’s usual rpy ordering. (If you ever swap to ‘zyx’, update the index accordingly.)
Time alignment tolerance
You use 50 ms (tolerance=0.05).
If your topics are high-rate (e.g., 100 Hz IMU, 30–100 Hz odom), 50 ms
can pair points a few frames apart and smear residuals. Try 10–20 ms:
pythontolerance=0.02
You’ll usually see slightly higher but truer residuals.
XY error for UKF is commented out
Re-enable it so you can compare EKF vs UKF on the same plot:
pythonplt.plot(df_merged["time_s"], df_merged["xy_error_raw"], label="UKF XY Error", linestyle="--", color="red")
Accuracy metric can be misleading
(1 - RMSE / range) * 100 is intuitive but can go negative/over 100 and ignores path length. Keep it, but also report path-length-normalized error:
pythonpath_len = np.sum(np.hypot(np.diff(df_merged["x_gt"]), np.diff(df_merged["y_gt"])))
pos_rmse_ekf = np.sqrt(np.mean((df_merged["x_gt"]-df_merged["x_odom"])**2 +
(df_merged["y_gt"]-df_merged["y_odom"])**2))
print(f"EKF Positional RMSE (2D): {pos_rmse_ekf:.4f} m | RMSE / path length: {100*pos_rmse_ekf/path_len:.3f}%")
Constant offset vs shape error
If start poses differ a little, RMSE includes that DC offset. For “shape” comparisons (e.g., circles and S-curves), optionally SE(2)-align est to GT before computing one of the RMSEs:
python# quick SE2 alignment: translate and rotate est to best match GT (least-squares on XY)
def align_se2(x_gt, y_gt, x_est, y_est):
X = np.vstack([x_gt, y_gt]).T
Y = np.vstack([x_est, y_est]).T
Xc, Yc = X - X.mean(0), Y - Y.mean(0)
U, _, Vt = np.linalg.svd(Yc.T @ Xc)
R2 = U @ Vt
if np.linalg.det(R2) < 0: # enforce proper rotation
U[:, -1] *= -1
R2 = U @ Vt
t = X.mean(0) - (R2 @ Y.mean(0))
Y_aligned = (R2 @ Y.T).T + t
return Y_aligned[:,0], Y_aligned[:,1]
Compute a second “shape-RMSE” on the aligned series and report both.
Innovation-style plots (very helpful for EKF≈UKF question)
Add residuals (GT − measurement used) where applicable. With odom, you can approximate:
pythonres_x_ekf = df_merged["x_gt"] - df_merged["x_odom"]
res_x_ukf = df_merged["x_gt"] - df_merged["x_raw"]
# same for y; then show mean≈0 and RMS vs your R
If residual stats hug your assumed R, your system is effectively linear and both filters will agree—this directly supports your conclusion.
Sector contribution helper
Your sector_contributions is correct but unused. Consider plotting it (for UKF & EKF) to localize where error dominates:
python# choose circle center
cx, cy = df_merged["x_gt"].mean(), df_merged["y_gt"].mean()
bins, sse_ukf, rmse_ukf = sector_contributions(df_merged["x_gt"], df_merged["y_gt"],
df_merged["x_raw"], df_merged["y_raw"], cx, cy)
bins, sse_ekf, rmse_ekf = sector_contributions(df_merged["x_gt"], df_merged["y_gt"],
df_merged["x_odom"], df_merged["y_odom"], cx, cy)
plt.figure()
plt.polar(bins, sse_ukf/np.sum(sse_ukf), label="UKF")
plt.polar(bins, sse_ekf/np.sum(sse_ekf), label="EKF", linestyle="--")
plt.legend(); plt.title("Sector SSE fraction")
plt.show()
Import duplication: you import numpy as np twice—harmless, but remove the second.
After merge_asof, dropna() can remove the very first/last rows (often where timing drifts). That’s fine; just be aware of edge trimming.
Save a figure before plt.show() (you do this correctly).
Consider resampling to a common time grid (e.g., 50 Hz) before error computations for apples-to-apples comparisons.
Add a compact summary that reviewers love:
pythonxy_rmse_ekf = np.sqrt(np.mean((df_merged["x_gt"]-df_merged["x_odom"])**2 +
(df_merged["y_gt"]-df_merged["y_odom"])**2))
xy_rmse_ukf = np.sqrt(np.mean((df_merged["x_gt"]-df_merged["x_raw"])**2 +
(df_merged["y_gt"]-df_merged["y_raw"])**2))
print(f"EKF: XY RMSE={xy_rmse_ekf:.3f} m | Yaw RMSE={np.degrees(rmse_position[2]):.2f}°")
print(f"UKF: XY RMSE={xy_rmse_ukf:.3f} m | Yaw RMSE={np.degrees(rmse_position[5]):.2f}°")
If you implement the (i) tighter timestamp tolerance, (ii) UKF XY-error plot, and (iii) optional SE2 alignment + path-length-norm metric, you’ll have a very clear, defensible comparison—and the residual plots will tell you immediately whether the controller/terrain are “too linear,” which explains EKF≈UKF.
Yep — and that’s actually a strong hint: your system is effectively linear enough (and well-tuned) that both filters make nearly the same predictions. In robot_localization, EKF and UKF share the same state + process model + covariances; UKF only pulls ahead when the measurement/propagation is noticeably nonlinear, biased, delayed, or non-Gaussian.
Here’s how to make (or detect) a difference—quick, actionable items:
Aggressive curvature changes: run fast S-turns with short segments and high |ω| (jerk in yaw).
3D coupling: drive up/down ramps so roll/pitch are nonzero while you still fuse yaw—this couples gravity into the IMU and breaks small-angle linearity.
Actuator limits: add motor saturation and deadzones (commanded ≠ achieved).
Slip bursts: inject intermittent lateral slip (vary μ/μ2 over time or add slip1/slip2 spikes on tracks).
Sensor delay & dropout: add 80–150 ms delay to mag yaw or drop 1/10th of messages; UKF generally handles this shape better with the same Q/R (or add a delay-compensating measurement model).
Yaw-rate bias drift: slowly vary gyro z-bias (e.g., random walk). UKF’s sigma points capture bias–angle coupling better.
Wheel/track scale error: 1–2% gain error on one side, plus occasional bias steps.
Misalignment: rotate the mag frame a few degrees relative to base; make the tilt-comp step slightly wrong.
Match rates: set UKF frequency: 100.0 like EKF so rate isn’t masking behavior.
Tune UKF sigma-point spread: expose α, β, κ (if available) or use the square-root UKF variant; poor spreads can make UKF look EKF-ish.
Increase Q during maneuvers: time-varying process noise (higher Q when |ω| or |a| are large) shows UKF’s advantage in propagating uncertainty through curvature.
Innovation statistics (NIS): log (y−H x̂)^T S^{-1} (y−H x̂) over time for each filter. If both sit near χ² bounds and look alike, the problem is quasi-linear.
State consistency (NEES): same idea but with state error vs covariance.
Residual histograms: EKF assumes Gaussian residuals; if you see skew/heavy tails (from slip/delay), UKF should outperform.
Sector/SSE plot you made: keep using it to localize where the error is born; then inject nonlinearity specifically in that sector.
Use raw mag yaw only (no yaw from IMU orientation) for the UKF or vice-versa; create a conflict between short-term (gyro) and long-term (mag) to force the filter to negotiate nonlinear wrapping and tilt-comp errors.
Enable vx, vy from odom with realistic covariances and occasionally corrupt one (short bursts). UKF cares more about cross-terms then.
Go 2D for a run (two_d_mode: true) to remove benign Z drift paths, then a 3D run with roll/pitch to induce coupling—compare both.
Tighter time merge: use 10–20 ms tolerance in your merge_asof.
Also report 2D positional RMSE (combined XY) and RMSE / path-length; differences sometimes hide when split into X vs Y.
Report a shape-only RMSE (SE(2) align est→GT, then compute RMSE) and a raw RMSE (includes bias). UKF often wins on the shape metric first.
If you try just three things first:
set UKF frequency = 100 Hz,
inject yaw-rate bias drift and mag delay/dropouts,
run fast alternating S-curves with occasional slip bursts,
…you should start to see UKF diverge (in a good way) from EKF on NIS/NEES, XY RMSE (shape-only), and heading during transients. If after that they’re still indistinguishable, that’s evidence your current operating envelope is well inside the linear regime—useful in itself for the paper.
Totally reasonable—and expected. Dropping steering_efficiency to 0.3 mainly changes how much yaw you get per command; it doesn’t actually introduce the kind of nonlinearity or sensor pathology that makes a UKF behave differently from an EKF in robot_localization.
Both filters still see essentially the same odom + IMU and the same
(nearly) linear kinematics, so they track almost identically.
If you want them to diverge (in a useful, diagnostic way), you need to change the estimation problem, not just the turn gain. The levers that matter most:
It scales commanded turning, but your measurement model (odom positions/vels + IMU rates/orientation) stays clean and time-aligned. The process model RL uses is close to linear in your operating range, so EKF linearization is fine and UKF’s sigma points bring no extra benefit.
Match update rates first
Set ukf_filter_node.frequency: 100.0 so rate isn’t masking behavior.
Introduce real nonlinearity or model mismatch
Rapid S-curves with high jerk: alternate ±ω with short segments (0.5–1.0 s) and occasional full stops/restarts.
3D coupling: drive on a ramp so roll/pitch ≠ 0 while yawing; keep fusing yaw. Small-angle assumptions break and coupling through gravity appears.
Asymmetric & time-varying friction: make left/right track μ and slip1/2 different and change them mid-run (e.g., μ_R drops 30% for 5 s).
Actuator limits: enable saturation/deadzones so cmd_vel_raw ≠ realized velocities.
Create sensor conditions UKF handles better
Yaw-rate bias drift: make gyro z-bias slowly wander (random walk). EKF linearization of bias–yaw coupling degrades sooner.
Measurement delay/jitter: add ~100 ms delay to /imu/mag_yaw (or sporadic dropouts).
Heading wrap events: include ±π crossings (fast 180° turns); UKF usually copes better with the nonlinearity.
Frame misalignment: offset mag frame by a few degrees to introduce a mild, persistent nonlinearity.
Different measurement mixes
For one filter, disable mag yaw (IMU yaw only); for the other, use mag yaw only for orientation (no IMU yaw), keeping gyro rates in both. This forces them to negotiate different nonlinearities.
Fuse vx, vy from odom (with realistic covariances) and occasionally corrupt one in bursts.
Tune the UKF to act “more UKF”
Expose/tune alpha, beta, kappa (if available in your RL UKF build). Start with α≈0.3–0.6, β=2, κ=0. Poor spreads can make a UKF behave EKF-ish.
Make sure your analysis can see differences
Tighten merge tolerance to 10–20 ms in merge_asof.
Plot XY error for UKF too (xy_error_raw) and 2D RMSE (√(Δx²+Δy²)).
Add shape-only RMSE (align est→GT in SE(2) before error) and raw RMSE.
Log innovations/NIS and, if possible, NEES. If both sit near χ² bounds and look the same, your scenario really is quasi-linear.
You already publish to /model/mmtr/cmd_vel_raw—good. Keep the controller plugin from reshaping your commands when you want raw dynamics.
Confirm the UKF really runs the same inputs as the EKF (same topics, same covariances, both getting mag yaw, etc.).
Ensure time stamps are consistent (no message header vs sim time mismatches).
Verify that odom you fuse is not near-ground-truth (some sim odom topics are very clean).
Bottom line: changing steering efficiency just scales the motion; it doesn’t inject the kind of error where a UKF shines. Add bias, delay, slip asymmetry, fast curvature changes, or 3D coupling—and match the UKF rate—and you’ll either (a) finally see the UKF pull ahead, or (b) have solid evidence your operating envelope is linear enough that an EKF is already optimal. Both outcomes are publishable.
Yep — that plot screams “systematic drift,” and it’s not surprising that EKF and UKF look the same. A near-linear rise in XY error with tiny yaw error usually means the filters are faithfully integrating a biased translational measurement (e.g., odom scale/slip), not getting tripped up by nonlinearity.
IMU yaw isn’t the culprit. If heading were the main issue you’d see oscillatory/cumulative yaw error that correlates with cross-track error. Here yaw tracks GT well while XY drifts almost linearly → classic velocity scale/slip bias or a steady frame misalignment.
EKF vs UKF parity is expected. With clean, near-linear kinematics and the same sensors, both estimators behave almost identically. Changing “steering_efficiency” doesn’t introduce the kind of nonlinearity that makes a UKF shine.
Plot UKF XY error too and overlay EKF: they’ll stack.
Cross- vs along-track error: project the position error onto the GT tangent/normal. A velocity scale bias shows as mostly along-track growth.
Slope of XY error vs time: constant slope ⇒ constant scale/slip bias.
SE(2) alignment: rigidly align the end pose (or best-fit similarity) and see residuals drop → again points to scale/heading offset, not estimator nonlinearity.
NIS/innovations (if you can log them): small, steady innovations in odom x/y with near-zero yaw innovation indicate odom bias is dominating.
Make the estimation problem less linear
Add gyro z-bias random walk; add mag yaw delay/jitter; drive with fast S-curves/stop-go; run on a ramp (roll/pitch coupling while yawing).
Introduce asymmetric, time-varying slip (μ/slip1/2 different left vs right, and change mid-run).
Alter the measurement mix
EKF: IMU yaw + odom (no mag). UKF: mag yaw + gyro rates (no IMU yaw orientation). Now they resolve different nonlinearities.
Fuse odom vx, vy with realistic covariances and occasionally corrupt one in bursts.
Match update rates: run UKF at 100 Hz so it’s not disadvantaged by frequency.
Bias estimation: augment the state with vx/vy (or wheel) scale factors or gyro bias and give them small process noise so the filter can absorb drift.
Use exteroception for loop closure even in sim (periodic LIDAR pose or beacons) to bound unobservable scale.
Add after df_merged is built:
python# Along-track (tangent) and cross-track (normal) errors
gt = df_merged[["x_gt","y_gt"]].values
est = df_merged[["x_odom","y_odom"]].values
dgt = np.gradient(gt, axis=0)
t_hat = dgt / (np.linalg.norm(dgt, axis=1, keepdims=True) + 1e-9) # unit tangent
n_hat = np.column_stack([-t_hat[:,1], t_hat[:,0]]) # unit normal
err = est - gt
along = np.sum(err * t_hat, axis=1)
cross = np.sum(err * n_hat, axis=1)
print(f"Mean |along|={np.mean(np.abs(along)):.3f} m, Mean |cross|={np.mean(np.abs(cross)):.3f} m")
If |along| » |cross|
and grows roughly linearly, you’ve got a scale/slip bias, not a yaw
problem — which also explains why EKF and UKF are twins here.
If your goal is to demonstrate divergence, pick one or two of the “make it nonlinear/pathological” levers above (e.g., gyro bias random walk + mag delay, or asymmetric, time-varying slip) and rerun. That will either (a) show UKF’s advantage, or (b) give you strong evidence that in your operating envelope an EKF is already sufficient — both are valuable results.
Short answer: you’re fine for local navigation and a robot-centric occupancy grid, but a purely dead-reckoned, long-horizon global map will warp unless you add some way to bound or correct the systematic drift.
Works well
Local planners & collision avoidance. You’re reacting to nearby obstacles; your yaw is tight, so local trajectory following and arc/circle primitives are OK.
Robot-centric (ego) occupancy grid. Keep the grid centered on the robot and scroll it with odom; cells age/decay. Relative obstacles are consistent even if the world frame drifts.
Struggles without correction
Global consistency. Systematic along-track drift makes the same place get painted in slightly different global positions → seams/ghost walls, especially on loops.
Global planning across large areas. If start and goal are far apart or require revisiting places, the map’s global frame errors can mislead the planner.
Use an ego grid + decay. 20–50 m window, probability decay and/or time-stamped cells. This matches your “relative occupancy” idea.
Propagate pose uncertainty into updates. Take the EKF/UKF pose covariance and inflate sensor noise or blur/inflate the update kernel accordingly. More uncertainty → softer updates → fewer hard ghosts.
Gate updates on reliability. If your slip detector/CNN or innovation checks say “odometry untrustworthy,” reduce update weight or pause mapping for a moment.
Exploit your collision signal. Treat contacts as high-confidence near-field obstacles; mark a short arc in front and behind (to handle stopping lag) with high occupancy, then decay quickly if not re-observed.
Submaps + pose graph (lightweight). Build small fixed submaps (e.g., 10–20 m chunks) in their own frames, then align overlapping submaps with scan matching (ICP/NDT/2D scan-to-submap). This bounds global drift while keeping your low-power sensor set.
Calibrate odom scale. From a straight-line GT run, fit a scale factor s so vtrue≈svodom; apply s online (or add s as a slowly varying state with small process noise).
Asymmetric slip model. Allow left/right scale factors to absorb constant curvature bias.
Occasional absolute yaw. Your mag yaw is already fused; ensure it’s not overly down-weighted at low dynamics so heading anchors long arcs.
Periodic soft resets. If you detect loop closure (scan match confidence high), nudge the pose or re-anchor the ego grid to reduce drift accumulation.
Along- vs cross-track error plot (the snippet I gave): shows drift is mostly along-track → classic odom scale/slip bias.
Repeat-pass consistency. Drive the same corridor twice; compute IoU (or correlation) between occupancy images after best SE(2) alignment. Report that ego-grid IoU stays high even as absolute pose drifts.
Map deformation with/without submaps. Small figure: single global grid (warps) vs submaps stitched (cleaner).
EKF/UKF can’t “invent” absolute position; with only IMU + wheel/track odom they’ll track heading well and let systematic translational bias leak into position.
That does not preclude a useful, realistic relative occupancy grid and safe local navigation.
If you need global consistency, add one light absolute cue (scan matching/AprilTag beacons/submaps) or estimate/correct the odom scale online.
Totally fair concern. If you plan to take this pipeline onto a real tracked robot, the question isn’t “can a relative grid work?” (yes), it’s “does my sim exercise the same failure modes I’ll see in the field?” Here’s a short, practical checklist to keep your approach realistic and transferable.
Robot-centric grid: Fine for local nav and for the “relative mapping” goal.
Filter config: Reasonable (IMU + yaw from mag + odom). Your yaw errors are tiny—good for near-term motion.
Error analysis: XY error over time + sector SSE plots are the right diagnostics.
IMU realism
Issue: Sim IMU often has fixed Gaussian noise + constant bias; real IMUs have bias random walk, scale factor, axis misalignment, temperature drift, and occasional spikes.
Do: Add bias random walk (Gauss–Markov) to gyro & accel; add small cross-axis terms and per-axis scale; inject rare outliers; vary bias with “temperature”.
Why: UKF/EKF behavior under slow bias wander is different from constant-bias sims.
Magnetometer realism
Issue: Sim field is perfect; real robots see hard/soft-iron offsets and local disturbances (motors, steel).
Do: Add a static 3×3 soft-iron matrix + hard-iron offset, plus intermittent disturbance bursts near the tracks/chassis. Periodically perturb yaw and watch the filter recover.
Why: Proves your yaw fusion and covariance gating are robust.
Timing & latency
Issue: In sim, everything is synchronized; real systems have sensor and actuation delays/jitter.
Do: Add 10–40 ms random latency to IMU/odom topics (and to cmd->motion). Mis-timestamp one stream by a few ms and ensure your pipeline handles it (or compensate in RL config).
Why: Even tiny misalignments change EKF/UKF innovations and can create apparent bias.
Odometry & actuation model
Issue: Tracked-vehicle plugins can be too linear: symmetric tracks, no backlash, no saturation, no left/right mismatch.
Do: Add asymmetry (e.g., 1–3% scale difference L/R), torque limits, backlash/dead-zone, and curvature-dependent slip (more slip at higher |ω|). Make friction spatially varying and slightly time-varying.
Why: Systematic along-track drift (what you see) should emerge from these effects, not from filter math alone.
Contact/friction
Issue: Constant μ, no compliance. Real ground has patches and debris.
Do: Use anisotropic friction (μ, μ2, slip1, slip2 already added—good); vary μ by ±20% over a grid; add small height noise/tilt to the floor; enable track compliance if available.
Why: Makes S-curves and arcs produce realistic slip-induced drift.
Frames & lever arms
Issue: In sim, sensor frames are perfectly aligned at the CoM.
Do: Offset IMU & mag a few cm and rotate by ~1–2°; include that transform in URDF. Give the filter the wrong transform by 0.5–1.0° and verify sensitivity.
Why: Real mounting errors matter for yaw/XY coupling during acceleration/rotation.
Covariances “honesty”
Issue: Covariances set by hand often don’t match injected noise → inconsistent filter (over-confident).
Do: Match the process & measurement covariances to your injected noise (or real sensor datasheet/Allan variance). Check that normalized innovation squared (NIS) ~ χ².
Why: Prevents NaNs/divergence and gives realistic residuals.
Controller realism
Issue: Velocity controller tracks commands too perfectly.
Do: Add rate limits, jerk limits, per-track gain mismatch, and a small command quantization. Optionally run a cascaded PID with saturation and anti-windup.
Why: Real controllers introduce lag that the filter must absorb.
No for local behavior. Your ego-centric occupancy grid with decay works well even with a slow along-track bias.
Yes for long/global alignment. If later you need stitching across long runs, add submaps (small local grids) and coarse scan-matching between submaps (even at low rate) to bound drift—still compatible with low-sensor setups.
Remove mag for 2–3 segments or increase its covariance briefly → watch yaw drift; UKF often handles the nonlinearity of aggressive S-turns slightly better.
Inject gyro bias steps (±0.2–0.5 °/s for a few seconds) → test bias observability; report recovery time.
Left/right odom scale split (+2%/−2%) → exposes curvature bias; show along-track error growth vs filter type and tuning.
Latency sweep (+/− 20 ms on IMU) → report RMSE sensitivity and NIS.
One paragraph describing simulated noise/bias models (random walk + misalignment + mag hard/soft iron).
A timing robustness note (added bounded timestamp jitter and command latency).
A systematic odom bias test (L/R asymmetry) with a small figure showing along-track error growth.
A statement that local, robot-centric OG works despite drift, and a note that global consistency would need submaps/loop closures.
If you implement even a subset of the above (bias random walk, mag distortions, timing offsets, L/R asymmetry), your results will read as credible and transferable—and you’ll likely see small but real differences between EKF and UKF on the S-curve stress tests.
Absolutely—here are drop-in ROS 2 nodes (Python) and minimal launch edits so you can:
gate (temporarily remove/de-weight) the mag yaw,
inject a gyro (yaw-rate) bias—either a timed step or a slow random walk, and
add timestamp latency/jitter to IMU (optional).
They’re small, self-contained, and designed to sit between your existing topics and the filters.
mag_gate.py — pass the message through most of the time, but during “off windows” either drop it or inflate covariance so RL ignores it.
python#!/usr/bin/env python3
import rclpy, math, ast
from rclpy.node import Node
from sensor_msgs.msg import Imu
class MagGate(Node):
def __init__(self):
super().__init__("mag_gate")
self.in_topic = self.declare_parameter("in_topic", "/imu/mag_yaw_raw").value
self.out_topic = self.declare_parameter("out_topic", "/imu/mag_yaw").value
self.mode = self.declare_parameter("mode", "inflate").value # "drop" or "inflate"
# windows: list of [start_s, end_s], e.g. [[10,18],[25,35]]
self.off_windows = ast.literal_eval(self.declare_parameter("off_windows", "[]").value)
self.inflated_var = float(self.declare_parameter("inflated_yaw_var", 1e6).value)
self.sub = self.create_subscription(Imu, self.in_topic, self.cb, 50)
self.pub = self.create_publisher(Imu, self.out_topic, 10)
def disabled_now(self, t):
for a,b in self.off_windows:
if a <= t <= b:
return True
return False
def cb(self, msg: Imu):
t = self.get_clock().now().nanoseconds * 1e-9
if self.disabled_now(t):
if self.mode == "drop":
return
# "inflate": keep publishing but make yaw useless
m = Imu()
m.header = msg.header
m.orientation = msg.orientation
cov = list(msg.orientation_covariance)
cov[8] = self.inflated_var
m.orientation_covariance = cov
m.angular_velocity = msg.angular_velocity
m.angular_velocity_covariance = msg.angular_velocity_covariance
m.linear_acceleration = msg.linear_acceleration
m.linear_acceleration_covariance = msg.linear_acceleration_covariance
self.pub.publish(m)
else:
self.pub.publish(msg)
def main():
rclpy.init()
n = MagGate()
rclpy.spin(n)
n.destroy_node()
rclpy.shutdown()
if __name__ == "__main__":
main()
Use it: set off_windows to intervals where you want the mag to be ignored (e.g., during parts of the S-curve).
imu_bias_injector.py — subscribes to your IMU for the filter (after your covariance override) and publishes a biased IMU. You can add a timed step bias and/or a random-walk bias.
python#!/usr/bin/env python3
import rclpy, math, random, ast
from rclpy.node import Node
from sensor_msgs.msg import Imu
class ImuBiasInjector(Node):
def __init__(self):
super().__init__("imu_bias_injector")
self.in_topic = self.declare_parameter("in_topic", "/imu/cov_fixed").value
self.out_topic = self.declare_parameter("out_topic", "/imu/for_filter").value
# Step bias (rad/s) applied in windows [[start,end],...]
self.step_bias_z = float(self.declare_parameter("step_bias_z", 0.0).value)
self.step_windows = ast.literal_eval(self.declare_parameter("step_windows", "[]").value)
# Random walk on bias (rad/s / sqrt(s))
self.rw_sigma = float(self.declare_parameter("rw_sigma", 0.0).value) # 0 disables
self.bias_rw_z = 0.0
self.last_t = None
self.sub = self.create_subscription(Imu, self.in_topic, self.cb, 200)
self.pub = self.create_publisher(Imu, self.out_topic, 50)
def step_active(self, t):
for a,b in self.step_windows:
if a <= t <= b:
return True
return False
def cb(self, msg: Imu):
now = self.get_clock().now().nanoseconds * 1e-9
if self.last_t is None:
self.last_t = now
dt = max(1e-3, now - self.last_t)
self.last_t = now
# Random-walk bias update
if self.rw_sigma > 0.0:
self.bias_rw_z += self.rw_sigma * math.sqrt(dt) * random.gauss(0.0, 1.0)
step = self.step_bias_z if self.step_active(now) else 0.0
total_bias_z = step + self.bias_rw_z
out = Imu()
out.header = msg.header
out.orientation = msg.orientation
out.orientation_covariance = msg.orientation_covariance
out.angular_velocity = msg.angular_velocity
out.angular_velocity.z = out.angular_velocity.z + total_bias_z # inject bias
out.angular_velocity_covariance = msg.angular_velocity_covariance
out.linear_acceleration = msg.linear_acceleration
out.linear_acceleration_covariance = msg.linear_acceleration_covariance
self.pub.publish(out)
def main():
rclpy.init()
n = ImuBiasInjector()
rclpy.spin(n)
n.destroy_node()
rclpy.shutdown()
if __name__ == "__main__":
main()
Typical values
Step: step_bias_z = 0.0052 (≈ 0.3°/s) during step_windows = [[12, 19]].
Random walk: rw_sigma = 5e-4 to 2e-3 rad/s/√s (tune to taste).
imu_delay.py — adds mean delay + uniform jitter to IMU timestamps/flow.
python#!/usr/bin/env python3
import rclpy, time, heapq, random
from rclpy.node import Node
from rclpy.duration import Duration
from sensor_msgs.msg import Imu
from rclpy.clock import ClockType
class ImuDelay(Node):
def __init__(self):
super().__init__("imu_delay")
self.in_topic = self.declare_parameter("in_topic", "/imu/for_filter").value
self.out_topic = self.declare_parameter("out_topic", "/imu/for_filter_delayed").value
self.mean_delay_ms = float(self.declare_parameter("mean_delay_ms", 20.0).value)
self.jitter_ms = float(self.declare_parameter("jitter_ms", 10.0).value)
self.sub = self.create_subscription(Imu, self.in_topic, self.cb, 200)
self.pub = self.create_publisher(Imu, self.out_topic, 50)
self.queue = [] # min-heap of (release_time, msg)
self.timer = self.create_timer(0.002, self.tick) # 500 Hz check
def cb(self, msg: Imu):
d = (self.mean_delay_ms + random.uniform(-self.jitter_ms, self.jitter_ms)) / 1000.0
release = self.get_clock().now().nanoseconds * 1e-9 + max(0.0, d)
heapq.heappush(self.queue, (release, msg))
def tick(self):
now = self.get_clock().now().nanoseconds * 1e-9
while self.queue and self.queue[0][0] <= now:
_, msg = heapq.heappop(self.queue)
# (Optionally) keep original stamp; most filters use header.stamp as-is.
self.pub.publish(msg)
def main():
rclpy.init()
n = ImuDelay()
rclpy.spin(n)
n.destroy_node()
rclpy.shutdown()
if __name__ == "__main__":
main()
You already have:
/imu/out → covariance_override.py → /imu/cov_fixed
mag node → /imu/mag_yaw (yaw-only IMU)
Let’s insert the new nodes and point RL at them.
Launch edits (high-level):
python# New nodes
mag_gate = Node(
package="mmtr_description",
executable="mag_gate.py",
parameters=[{
"in_topic": "/imu/mag_yaw_raw",
"out_topic": "/imu/mag_yaw",
"mode": "inflate", # or "drop"
"off_windows": "[[12,20],[32,42]]",
"inflated_yaw_var": 1e6
}]
)
imu_bias = Node(
package="mmtr_description",
executable="imu_bias_injector.py",
parameters=[{
"in_topic": "/imu/cov_fixed",
"out_topic": "/imu/for_filter",
"step_bias_z": 0.0052, # ~0.3 deg/s
"step_windows": "[[12,19]]",
"rw_sigma": 0.001 # rad/s/sqrt(s)
}]
)
imu_delay = Node( # optional
package="mmtr_description",
executable="imu_delay.py",
parameters=[{
"in_topic": "/imu/for_filter",
"out_topic": "/imu/for_filter_delayed",
"mean_delay_ms": 20.0,
"jitter_ms": 10.0
}]
)
Then point EKF/UKF to the delayed (or biased) IMU and gated mag:
In ekf.yaml / ukf.yaml:
imu0: /imu/for_filter_delayed (or /imu/for_filter if you skip the delay)
imu1: /imu/mag_yaw (output of mag_gate.py)
If you’d rather “remove mag” without new nodes: you can set
imu1_configyaw tofalseor setorientation_covariance[8]very high in yourmag_yaw_pub. The gate node just makes it easy to switch during a run.
Mag blackout + S-curve
Use mag_gate.off_windows="[[10,25]]" during the high-curvature section.
Keep gyro rw_sigma ~ 1e-3 and a small step_bias_z ~ 0.003 rad/s overlapping the blackout.
Expect: steady yaw drift during blackout; UKF may track slightly better through the nonlinearity; show XY error increase & recovery when mag returns.
Latency sweep
Run three trials with mean_delay_ms = 0, 20, 40 (keep jitter 10 ms).
Plot RMSE_x/y/ψ and NIS if you log innovations.
Expect: degradation with delay; sometimes UKF is a bit more tolerant when motion is highly curved.
(Optional) “No-mag” baseline
Disable mag entirely (empty off_windows + set very large yaw var in mag node or comment imu1).
Demonstrates you need mag (or another yaw absolute) for long arcs.
You’ll be able to prove the filters are robust when the mag is unreliable (real-world common), and you’ll see where systematic odom slip dominates.
You’ll expose realistic failure modes (bias wander + timing) and can quantify recovery time and residual drift—great material for the paper.
If you want, I can also give you a tiny odom delay/bias node (to simulate encoder scale mismatch) so the position source fed to RL is mildly wrong—another classic way to separate EKF/UKF behavior.
If you want to combine both a mag yaw gater and a cmd_vel wrapper into your pipeline, here’s how they would work and why they might be useful in your case:
mag_yaw_gaterWe basically already covered this — it sits between /imu/mag_yaw_raw and what the EKF/UKF sees as /imu/mag_yaw.
Purpose:
Simulate loss or de-weighting of magnetometer yaw during certain time windows.
Create realistic divergence between EKF and UKF by forcing them to rely on gyro integration alone.
Placement:
bash/imu/mag_yaw_raw → mag_yaw_gater.py → /imu/mag_yaw
/imu/mag_yaw then goes into EKF/UKF as an absolute yaw measurement.
cmd_vel_wrapperThis is separate but complementary — it wraps /model/mmtr/cmd_vel_raw (your true command) and outputs a modified /model/mmtr/cmd_vel that is actually sent to the sim’s tracked vehicle controller.
Purpose:
Simulate imperfect actuation:
reduced steering efficiency
wheel slip / track slip
deadband
latency in commands
Force trajectory to deviate slightly from plan → amplify the effect of mag blackout or gyro bias.
Example node (cmd_vel_wrapper.py):
python#!/usr/bin/env python3
import rclpy, math, random
from rclpy.node import Node
from geometry_msgs.msg import Twist
class CmdVelWrapper(Node):
def __init__(self):
super().__init__("cmd_vel_wrapper")
self.in_topic = self.declare_parameter("in_topic", "/model/mmtr/cmd_vel_raw").value
self.out_topic = self.declare_parameter("out_topic", "/model/mmtr/cmd_vel").value
self.steering_scale = float(self.declare_parameter("steering_scale", 1.0).value)
self.linear_scale = float(self.declare_parameter("linear_scale", 1.0).value)
self.cmd_delay_s = float(self.declare_parameter("cmd_delay_s", 0.0).value)
self.noise_lin_std = float(self.declare_parameter("noise_lin_std", 0.0).value)
self.noise_ang_std = float(self.declare_parameter("noise_ang_std", 0.0).value)
self.pub = self.create_publisher(Twist, self.out_topic, 10)
self.sub = self.create_subscription(Twist, self.in_topic, self.cb, 10)
self.buffer = []
self.timer = self.create_timer(0.02, self.tick) # 50 Hz
def cb(self, msg):
# Apply scaling
new_msg = Twist()
new_msg.linear.x = self.linear_scale * msg.linear.x
new_msg.angular.z = self.steering_scale * msg.angular.z
# Apply noise
new_msg.linear.x += random.gauss(0.0, self.noise_lin_std)
new_msg.angular.z += random.gauss(0.0, self.noise_ang_std)
# Delay buffer
release_time = self.get_clock().now().nanoseconds * 1e-9 + self.cmd_delay_s
self.buffer.append((release_time, new_msg))
def tick(self):
now = self.get_clock().now().nanoseconds * 1e-9
self.buffer = [(t, m) for t, m in self.buffer if not self._try_publish(t, m, now)]
def _try_publish(self, release_time, msg, now):
if now >= release_time:
self.pub.publish(msg)
return True
return False
def main():
rclpy.init()
node = CmdVelWrapper()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == "__main__":
main()
Right now, your EKF and UKF outputs are too similar because:
The robot moves exactly as commanded.
The mag yaw is always available and perfectly correct.
By wrapping both:
cmd_vel_wrapper makes motion imperfect → small systematic drift starts accumulating.
mag_yaw_gater removes the "absolute yaw fix" during part of the run → drift is uncorrected and becomes visible.
EKF vs UKF will start to diverge in yaw and XY position, because they propagate and fuse differently.
Typical launch wiring:
pythoncmd_vel_wrapper = Node(
package="mmtr_description",
executable="cmd_vel_wrapper.py",
parameters=[{
"in_topic": "/model/mmtr/cmd_vel_raw",
"out_topic": "/model/mmtr/cmd_vel",
"steering_scale": 0.9,
"linear_scale": 0.98,
"noise_lin_std": 0.01, # m/s
"noise_ang_std": 0.002, # rad/s
"cmd_delay_s": 0.05
}]
)
mag_gate = Node(
package="mmtr_description",
executable="mag_yaw_gater.py",
parameters=[{
"in_topic": "/imu/mag_yaw_raw",
"out_topic": "/imu/mag_yaw",
"mode": "inflate",
"off_windows": "[[10, 20], [35, 50]]"
}]
)
If we combine gyro bias injection from before with cmd_vel_wrapper + mag yaw gating, you’ll get:
Systematic drift when mag is unavailable
Motion mismatch between ground truth and odometry
UKF and EKF starting to behave differently → more realistic results for your planned real-robot pipeline.
Do you want me to merge gyro bias injection into the same node as cmd_vel_wrapper so it’s all in one place, or keep them separate for clarity?
Great —this is close, but a few wiring/param mismatches will make the results confusing. Here’s a focused checklist + tiny patches so the graph is unambiguous and you actually exercise the stressors.
Pick ONE mag gater.
You launch both mag_yaw_gated.py and mag_gate.py, and both publish to /imu/mag_yaw. That will race.
→ Keep mag_gate.py (the one with mode/off_windows), and remove mag_yaw_gated.py from the launch.
Make the IMU chain explicit, then point EKF/UKF at it.
Desired flow:
bash/imu/out ──> covariance_override.py ──> /imu/cov_fixed ──> imu_bias_injector.py ──> /imu/for_filter ──> imu_delay.py ──> /imu/for_filter_delayed
Then set both filters’ imu0: to /imu/for_filter_delayed. (Right now your ekf/ukf YAMLs still say /imu/cov_fixed.)
Mag path: yaw from the (delayed) IMU orientation.
Your mag_yaw_pub.py defaults to imu_topic="/imu/out_fixed_cov" unless you override it. In the launch, you don’t set that param, so it may be reading the wrong IMU.
→ Pass imu_topic="/imu/for_filter_delayed" and out_topic="/imu/mag_yaw_raw" to mag_yaw_pub.py, then gate it to /imu/mag_yaw.
cmd_vel_wrapper params mismatch.
The node you pasted earlier expects:
in_topic, out_topic, steering_scale, linear_scale, cmd_delay_s, noise_lin_std, noise_ang_std
But you’re passing: delay_s, yaw_slew, slip_time, slip_lin_scale, slip_yaw_scale. Those won’t do anything.
→ Either update the node to use those names, or (simpler) pass the params it actually expects.
Run UKF at the same rate as EKF.
In ukf.yaml: set frequency: 100.0 (to match your EKF) so rate doesn’t hide differences.
python# REMOVE this block entirely (duplicate/conflict)
# mag_yaw_gater = Node(
# package="mmtr_description",
# executable="mag_yaw_gated.py",
# parameters=[{'yaw_var_nom': 0.02, 'gate_secs': 2.0, 'rate_gate': 0.8}],
# )
# Keep this one (mag gate with windows)
mag_gate = Node(
package="mmtr_description",
executable="mag_gate.py",
parameters=[{
"in_topic": "/imu/mag_yaw_raw",
"out_topic": "/imu/mag_yaw",
"mode": "inflate", # or "drop"
"off_windows": "[[12,20],[32,42]]",
"inflated_yaw_var": 1e6
}]
)
pythoncov_overide = Node(
package="mmtr_description",
executable="covariance_override.py",
# ensure this node subscribes /imu/out and publishes /imu/cov_fixed
)
imu_bias = Node(
package="mmtr_description",
executable="imu_bias_injector.py",
parameters=[{
"in_topic": "/imu/cov_fixed",
"out_topic": "/imu/for_filter",
"step_bias_z": 0.0052, # ~0.3 deg/s
"step_windows": "[[12,19]]",
"rw_sigma": 0.001 # rad/s/sqrt(s)
}]
)
imu_delay = Node(
package="mmtr_description",
executable="imu_delay.py",
parameters=[{
"in_topic": "/imu/for_filter",
"out_topic": "/imu/for_filter_delayed",
"mean_delay_ms": 20.0,
"jitter_ms": 10.0
}]
)
mag_yaw_pub = Node(
package="mmtr_description",
executable="mag_yaw_pub.py",
parameters=[{
"imu_topic": "/imu/for_filter_delayed", # <-- IMPORTANT
"out_topic": "/imu/mag_yaw_raw", # publish raw yaw before gate
"yaw_variance": 0.02,
"roll_pitch_cov": 99999.0
}]
)
pythoncmd_vel_wrapper = Node(
package="mmtr_description",
executable="cmd_vel_wrapper.py",
parameters=[{
"in_topic": "/model/mmtr/cmd_vel_raw",
"out_topic": "/model/mmtr/cmd_vel",
"linear_scale": 0.98,
"steering_scale": 0.90,
"cmd_delay_s": 0.05,
"noise_lin_std": 0.01, # m/s
"noise_ang_std": 0.002 # rad/s
}]
)
In ekf.yaml and ukf.yaml:
yamlimu0: /imu/for_filter_delayed
imu1: /imu/mag_yaw
frequency: 100.0 # (for UKF too)
Keep your imu0_config / imu1_config as before (imu0: full orientation + angular rates, imu1: yaw-only).
ros2 topic hz /imu/for_filter_delayed ~100 Hz; …/mag_yaw ~30 Hz.
ros2 topic echo /imu/mag_yaw | grep orientation_covariance during your gate windows → last entry jumps to ~1e6 (inflate) or messages drop (mode=drop).
A quick log of angular velocity z before/after imu_bias_injector.py shows the bias step/random-walk.
Confirm only one publisher on /imu/mag_yaw (ros2 topic info /imu/mag_yaw).
During mag gate windows, yaw becomes gyro-integrated (with bias + IMU delay), so heading drifts; with the cmd_vel wrapper your path also deviates slightly from commands.
That combination is the realistic stress you wanted: you should see divergence EKF vs UKF in yaw and XY (especially during the gated intervals), and recovery when mag returns.
If you want me to bundle cmd_vel_wrapper with slip “events” (e.g., 0.5 s bursts where linear_scale→0.85 and steering_scale→0.75), I can sketch that too.
Nice —this is almost there. A few small but important tweaks so the UKF actually “feels” your injected delays/bias and doesn’t over-constrain yaw:
Right now you have:
yamlimu0_config: [false,false,false, true,true,true, ..., true,true,true, ...]
imu1_config: [..., false,false,true, ...] # yaw-only mag
imu0_config’s orientation triplet true,true,true means roll, pitch, AND yaw
from the IMU orientation are treated as absolute measurements. Since
you also feed yaw from the magnetometer, yaw becomes double-constrained
and the filter will mostly split the difference (often hiding the
effects you’re trying to expose).
Fix (recommended): make IMU provide roll & pitch only, plus all angular rates; leave absolute yaw to the magnetometer.
yamlimu0_config: [false,false,false,
true, true, false, # roll, pitch only; NO yaw
false,false,false,
true, true, true,
false,false,false]
(Alternative: keep IMU yaw “true” but inflate its yaw covariance significantly vs mag; the above is cleaner.)
nodelay with your injected delayYou inject transport delay with /imu/for_filter_delayed while keeping the original header stamp. That means the message arrives late relative to its stamp.
If you keep imu0_nodelay: true,
RL will fuse the message on receipt without time compensation, which is
less realistic (it treats the delayed sample as if it were current).
Better: set
yamlimu0_nodelay: false
so RL uses the message timestamp for time alignment. Also ensure your sensor_timeout (default 0.1 s) is > your max injected delay+jitter.
(Alternative: in your delay node, also shift header.stamp forward to “now” on publish and keep nodelay: true. Either approach is fine; just be consistent.)
You’ve already pointed UKF to /imu/for_filter_delayed. Do the same in EKF, and keep imu1: /imu/mag_yaw for both. Run both at frequency: 100.0. That way any divergence is down to model/linearization, not differing inputs.
Before running the stress test:
ros2 topic info /imu/mag_yaw → exactly one publisher (your gater).
ros2 topic hz /imu/for_filter_delayed ~100 Hz; /imu/mag_yaw ~30 Hz.
During a gate window, confirm yaw covariance inflates (or messages drop) so yaw comes only from gyro integration.
If IMU yaw is disabled as above, keep imu0 orientation cov for roll/pitch realistic (e.g., ~0.001–0.01 rad²), and keep imu1 yaw variance near your mag_yaw_pub’s advertised value (~0.02 rad²) except when gated (then 1e6).
yamlimu0: /imu/for_filter_delayed
imu0_config: [false,false,false,
true, true, false, # <- yaw FALSE
false,false,false,
true, true, true,
false,false,false]
imu0_nodelay: false # <- handle injected delay via timestamps
imu1: /imu/mag_yaw # yaw-only mag, as you have
imu1_config: [false,false,false,
false,false,true,
false,false,false,
false,false,false,
false,false,false]
frequency: 100.0
Make the same edits in the EKF file too. After that, your mag blackout + gyro bias + cmd_vel wrapper should finally reveal the EKF/UKF differences you wanted.
This is very close to what we want. You’ve done the big bits right: IMU yaw removed (roll/pitch only + all gyro rates), IMU delay handled with imu0_nodelay: false, and mag yaw coming on /imu/mag_yaw.
A few small but important fixes:
Add the missing imu1 flags back (be explicit):
yamlimu1: /imu/mag_yaw
imu1_config: [false,false,false,
false,false,true,
false,false,false,
false,false,false,
false,false,false]
imu1_differential: false
imu1_remove_gravitational_acceleration: false
Don’t fuse Z (and probably VZ) from odom unless you truly have it. Your odom0_config currently has Z & VZ enabled:
yaml# current
odom0_config: [true, true, true, # <- Z is true here
false,false,false,
true, true, true, # <- VZ is true here
false,false,false,
false,false,false]
For a planar tracked robot, use:
yaml# recommended
odom0_config: [true, true, false, # X,Y only
false,false,false, # no roll/pitch/yaw pose
true, false, false, # VX only (set VY true only if you *trust* it)
false,false,false,
false,false,false]
Remove the duplicate frequency under the imu1 block. You already set frequency: 100.0 at the top; keep just one.
Set a sensor_timeout big enough to tolerate your injected IMU/mag delays and blackout windows:
yamlsensor_timeout: 0.5
(Adjust if your mag gate “drop” windows are longer; this prevents spurious timeouts.)
Match EKF settings to these same topics/flags so any divergence you see is due to filter structure, not different inputs.
In UKF only, slightly increase process noise on yaw rate (vyaw) and maybe lateral velocity (vy) to let it express nonlinearity during mag blackouts:
e.g. in UKF process_noise_covariance, bump vyaw from 8e-1 → 1.2 and vy from 2e-1 → 3e-1.
Keep EKF’s Q as-is. This often makes the UKF drift a bit more (and recover faster when mag returns), giving you the divergence you want to document.
ros2 topic info /imu/mag_yaw → exactly one publisher (your gater).
During gate windows, /imu/mag_yaw either drops messages or advertises a huge yaw variance (if “inflate” mode).
/imu/for_filter_delayed at ~100 Hz; UKF imu0_nodelay: false (as you set) uses timestamps correctly.
If you apply the odom0_config change and the missing imu1_* flags, you should be in great shape to see meaningful divergence during mag blackouts with gyro bias + cmd_vel imperfections.
Short answer: use only one—not both.
If you want simple, deterministic gating (e.g., “turn mag off between 12–20 s and 32–42 s” or inflate its covariance there), use mag_gate.py. That’s the one you already parameterized with mode, off_windows, and inflated_yaw_var.
Wire: /imu/mag_yaw_raw → mag_gate.py → /imu/mag_yaw
If you want adaptive gating (e.g., drop/inflate mag when it disagrees with gyro-integrated yaw or when turn-rate is high), then use mag_yaw_gated.py (your “gater” with gate_secs, rate_gate, etc.).
Wire: /imu/mag_yaw_raw → mag_yaw_gated.py → /imu/mag_yaw
But do not run both. Pick one based on the behavior you need:
Reproducible stress windows for experiments/paper: pick mag_gate.py.
Realism (automatic rejection during disturbances): pick mag_yaw_gated.py.
Whichever you choose, verify:
nginxros2 topic info /imu/mag_yaw # should show exactly 1 publisher (your chosen gater)
And keep EKF/UKF imu1: /imu/mag_yaw consistent.
I don’t see an imu_delay.py in the code you’ve pasted — it looks like it’s something you intended to have in your package (mmtr_description) but hasn’t been created yet.
From your launch file, you had:
pythonimu_delay = Node(
package="mmtr_description",
executable="imu_delay.py",
parameters=[{
"in_topic": "/imu/for_filter",
"out_topic": "/imu/for_filter_delayed",
"mean_delay_ms": 20.0,
"jitter_ms": 10.0
}]
)
That means ROS 2 will try to run a Python executable called imu_delay.py from the mmtr_description package’s scripts or src folder — but unless you’ve written that script, it doesn’t exist.
If you want it, I can write a minimal imu_delay.py for you that:
Subscribes to an input topic (e.g., /imu/for_filter)
Buffers messages for a given mean delay + random jitter
Publishes them to the output topic (/imu/for_filter_delayed)
Do you want me to create that delay node? That way your simulated IMU can have timing latency before hitting the EKF/UKF.
Yes — earlier when we discussed adding realism with sensor imperfections, I suggested an imu_delay.py node along with imu_bias_injector.py and mag_gate.py.
I outlined it but we didn’t actually paste the full code into your workspace yet.
If you want, I can now give you a working imu_delay.py that:
Subscribes to an input IMU (or any message type)
Delays publishing by a configurable mean_delay_ms and jitter_ms
Preserves timestamps
Works for ROS 2 Python nodes
This would let you simulate timing delays before the data reaches the EKF/UKF — which can help cause divergence between EKF and UKF in your tests.
Do you want me to give you that complete imu_delay.py now?
Here’s
a compact ROS 2 (rclpy) node that delays IMU messages by a configurable
mean delay with optional jitter. It preserves the original header.stamp by default (good if your filter uses imu0_nodelay: false), but you can optionally retimestamp to “now”.
Save as imu_delay.py in your mmtr_description package (e.g., mmtr_description/scripts/imu_delay.py) and make it executable.
python#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy
from rclpy.time import Time
from sensor_msgs.msg import Imu
import heapq
import random
import copy
class ImuDelay(Node):
def __init__(self):
super().__init__('imu_delay')
# -------- Parameters --------
self.in_topic = self.declare_parameter('in_topic', '/imu/for_filter').value
self.out_topic = self.declare_parameter('out_topic', '/imu/for_filter_delayed').value
self.mean_delay_ms = float(self.declare_parameter('mean_delay_ms', 20.0).value) # e.g. 20 ms
self.jitter_ms = float(self.declare_parameter('jitter_ms', 10.0).value) # e.g. ±10 ms
self.retimestamp = bool(self.declare_parameter('retimestamp', False).value) # overwrite header.stamp with publish time?
self.drop_rate = float(self.declare_parameter('drop_rate', 0.0).value) # e.g. 0.02 = 2% drops
self.max_queue_sec = float(self.declare_parameter('max_queue_sec', 0.5).value) # safety bound
# Sensor-data QoS
qos = QoSProfile(
reliability=ReliabilityPolicy.BEST_EFFORT,
history=HistoryPolicy.KEEP_LAST,
depth=10,
durability=DurabilityPolicy.VOLATILE
)
self.sub = self.create_subscription(Imu, self.in_topic, self.cb, qos)
self.pub = self.create_publisher(Imu, self.out_topic, qos)
# Priority queue of (due_time_ns, seq, Imu)
self._pq = []
self._seq = 0
# Timer to flush queue
self.timer = self.create_timer(0.001, self.flush) # 1 kHz check
self.get_logger().info(
f"imu_delay: {self.in_topic} -> {self.out_topic} | mean={self.mean_delay_ms} ms, "
f"jitter=±{self.jitter_ms} ms, retimestamp={self.retimestamp}, drop_rate={self.drop_rate}"
)
def cb(self, msg: Imu):
# Optional random drop
if self.drop_rate > 0.0 and random.random() < self.drop_rate:
return
# Schedule publish time
now = self.get_clock().now()
jitter = (random.uniform(-self.jitter_ms, self.jitter_ms) * 1e6) # ns
delay_ns = int(self.mean_delay_ms * 1e6 + jitter)
delay_ns = max(0, delay_ns)
due_time = now.nanoseconds + delay_ns
# Safety: don’t hold messages forever
due_time = min(due_time, now.nanoseconds + int(self.max_queue_sec * 1e9))
# Deep copy so later modifications (e.g., retimestamp) don’t affect queued references
to_queue = copy.deepcopy(msg)
# Enqueue
heapq.heappush(self._pq, (due_time, self._seq, to_queue))
self._seq += 1
def flush(self):
if not self._pq:
return
now_ns = self.get_clock().now().nanoseconds
# Publish all messages whose due_time has arrived
while self._pq and self._pq[0][0] <= now_ns:
_, _, msg = heapq.heappop(self._pq)
if self.retimestamp:
# Overwrite to "now"; useful if your filter uses imu0_nodelay: true
msg.header.stamp = self.get_clock().now().to_msg()
self.pub.publish(msg)
def main(args=None):
rclpy.init(args=args)
node = ImuDelay()
try:
rclpy.spin(node)
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
pythonimu_delay = Node(
package="mmtr_description",
executable="imu_delay.py",
parameters=[{
"in_topic": "/imu/for_filter",
"out_topic": "/imu/for_filter_delayed",
"mean_delay_ms": 20.0,
"jitter_ms": 10.0,
"retimestamp": False, # keep original header.stamp
"drop_rate": 0.0
}]
)
With retimestamp=False, keep imu0_nodelay: false in your EKF/UKF so fusion uses message timestamps (this is the realistic pipeline you set up).
If you decide to set retimestamp=True, then set imu0_nodelay: true so the filter treats arrivals as “current.”
drop_rate lets you simulate packet loss if you need more stress.
Totally fair frustration. Two things are happening:
the NaNs mean the UKF has become numerically inconsistent (bad conditioning / unobservable mix), and
your pipeline still makes the system so close to linear / well-observed that EKF≈UKF most of the time.
Below is a tight checklist that fixes the NaNs first, then—only after it’s stable—pushes the system to actually show EKF vs UKF differences.
A. Don’t fuse unobservable states
Your last YAML still had Z & VZ fused at some point. For a planar robot with no Z sensor, this often causes blow-ups.
yamlodom0_config: [ true, true, false, # X,Y only
false,false,false, # no roll/pitch/yaw pose
true, false, false, # VX only (VY only if you *really* trust it)
false,false,false,
false,false,false ]
B. Ensure IMU yaw is NOT fused when mag is your yaw
(You already did this, keep it.)
yamlimu0_config: [false,false,false,
true, true, false, # roll,pitch only
false,false,false,
true, true, true,
false,false,false]
C. Keep IMU delay consistent with fusion timing
Since you delay IMU and keep the original header.stamp, use:
yamlimu0_nodelay: false
sensor_timeout: 0.5 # give yourself headroom vs delay/jitter
D. Covariances must be sane, symmetric, positive
Don’t set any measurement covariance to 0 (or extremely tiny) unless you’re sure.
For mag yaw, advertise e.g. 0.02 rad² nominal; inflate to ~1e6 when gated (or drop the msg).
If you enabled VY at any point, give it a large covariance or just turn it off (A).
E. Increase process noise where you injected non-idealities
You injected gyro bias and cmd_vel imperfections; reflect that in Q, or the filter will “fight” the data and blow up.
UKF vyaw (yaw rate) process noise: increase (e.g. from 0.8 → 1.2).
If you allow side slip, UKF vy process noise: increase (e.g. 0.2 → 0.4).
F. Initial covariance not too confident
If you start with very tight initial yaw/vel covariances, a
delayed/mismatched measurement can spike the gain. Loosen a bit (e.g.,
yaw 0.2–0.5 rad², vx 0.5 m²/s²).
G. Verify the inputs aren’t NaN
Log the three feeds when the UKF dies:
bashros2 topic echo -n 1 /imu/for_filter_delayed
ros2 topic echo -n 1 /imu/mag_yaw
ros2 topic echo -n 1 /model/mmtr/odom_cov_fixed
Any .nan sneaking in will immediately propagate.
H. Keep two_d_mode=false (fine) but do not fuse Z; or set two_d_mode: true if you prefer hard planar constraints (then remove Z/roll/pitch fusions).
Do A–H and the NaNs should disappear.
Right now your system is still “too linear / too well corrected”:
mag yaw at 30 Hz + small heading errors + modest velocities ⇒ almost linear;
both filters share the same measurement models & covariances ⇒ they behave alike.
To force a difference (after it’s numerically stable):
Create yaw observability gaps
Gate mag for longer windows (e.g. 8–12 s) while turning; or drop mag when |gyro| > threshold.
UKF (sigma points) tends to recover better when you reintroduce a highly nonlinear yaw correction after drift; EKF may under/over-correct.
Increase nonlinearity
Use higher-rate S-curves with varying curvature (e.g. sinusoidal ω with occasional spikes).
Add cmd_vel delay/jitter and a small random slip in VY during turns (you already have a wrapper—turn up slip_yaw_scale and slip_lin_scale, and add intermittent lateral slip bursts).
Make UKF more “expressive” than EKF
Only in UKF, bump Q_vyaw and (if used) Q_vy as above. Keep EKF Q smaller.
Keep the same measurement covariances in both. Now process model differences matter.
Lower mag rate temporarily
Drop mag to 5–10 Hz for some runs. With lower yaw corrections, continuous turning accentuates process-model differences.
Evaluation that highlights divergence
Plot segment-wise RMSE (per 90° or per 10 s window).
Plot innovation (residual) norm over time for yaw & XY (EKF and UKF). UKF should show different innovation behavior during re-lock after blackout.
Your sector contribution plot around circles is great—do it for each lap and compare EKF vs UKF peaks.
Fix odom0_config to not fuse Z and VZ (A).
Keep imu0_nodelay: false, add sensor_timeout: 0.5 (C).
Increase UKF vyaw Q to 1.2; optionally vy Q to 0.4 (E).
Gate mag for 8–12 s while turning (1).
Leave EKF Q as-is.
If NaNs persist after A–H, paste the exact odom0_config, imu0_config, and a one-second snippet of the three input topics right before the crash—I’ll zero in on the culprit.
Short answer: yes, the controller is a big part of why EKF and UKF look the same—and you still have levers you can pull even if you can’t rewrite it.
The Gazebo TrackedVehicle system is fairly idealized: it maps cmd_vel
→ track velocities → body twist with simple kinematics, then your odom
integrates that almost perfectly. If odom is “too clean,” and mag yaw
keeps heading bounded, both filters get nearly the same, easy problem.
Systematic errors from the controller (e.g., constant steering efficiency) appear as repeatable bias, which the two filters address similarly when they have the same sensors/covariances.
Check what odom really is
Echo /model/mmtr/odometry (or your cov-fixed version) and compare its twist.twist.angular.z to IMU gyro z. If they match almost perfectly during turns, your odom is effectively ground truth -> filters won’t diverge.
Compare commanded vs achieved
Log cmd_vel vs odom twist. If tracking is tight (small lag/overshoot), the dynamics are too linear/ideal.
Turn mag off for long windows while turning
If even then EKF≈UKF, it’s because odom dominates and is too clean.
All of these make the estimation problem more realistic and expose EKF/UKF differences:
Use “encoder odom” instead of Gazebo’s perfect odom
Build odom from per-track speeds (from the TrackController joints) with finite resolution, bias, and slip.
Inject a curvature-dependent covariance: inflate pose/twist cov when |ω| is high or when track speeds differ a lot.
Dynamic covariance scaling (cheap and effective)
Wrap your odom with a node that increases pose.covariance and twist.covariance when:
|ω|/|v| exceeds a threshold (tight turning),
IMU yaw rate disagrees with odom yaw rate by > γ,
your slip heuristic triggers (e.g., large lateral accel with small commanded vy).
EKF (linearized) vs UKF (sigma points) will weight measurements differently once covariances move.
Mag gating with observability gaps
Keep your mag_gate/mag_yaw_gated node and remove yaw for 8–12 s during curves.
Now heading is carried by the process model + gyro bias; UKF typically handles the re-lock nonlinearity better.
Timing realism on sensors/actuation
Keep imu_delay.py in the chain (imu0_nodelay: false in the UKF/EKF).
Add your cmd_vel_wrapper (slew rate, latency, occasional slip bursts) so odom is no longer perfectly predictable from commands.
Heterogeneous terrain & small slopes
Patch the world with different μ and slip1/2 patches or a 1–2° tilt. Tracked vehicles show clear, localized slip—your sector RMSE will light up and filters will diverge.
Limit feedback frequency or add saturation
Reduce odom pub rate to, say, 20–30 Hz (keep IMU at 100 Hz).
Add rate/accel saturation in your wrapper to mimic motor/current limits; this breaks ideal kinematics.
Make UKF “more expressive” than EKF
Increase UKF process noise for vyaw (and vy if you use it) so it can represent slip and bias better; keep EKF Q smaller.
Keep measurement R identical across filters so the divergence is attributable to the process model.
Add small deadband, backlash, and per-track gain mismatch (±5–10%).
Add command-to-track velocity delay and limited jerk (you already have jerk limits—accentuate them).
No—your stated goal is a relative map updated by contact/collision cues. That’s robust to slow drift; what matters is that short-term local pose is consistent over the distance between contacts, and you’ve already shown sub-meter XY RMSE and sub-degree yaw RMSE on long circles. The realism question isn’t “are EKF and UKF identical,” it’s “does the estimator’s uncertainty grow in the places a real robot would struggle?” With the tweaks above (dynamic covariances, mag gaps, timing/slip), you’ll see that—and your paper can justify realism.
Keep IMU delay (imu0_nodelay: false) and mag gating for 10 s during turns.
Add an odom_cov_scaler node: inflate cov when |ω|>0.6 rad/s or when |gyro_z - odom_ωz|>0.1 rad/s.
Raise UKF Q_vyaw (e.g., 0.8→1.2) and optionally Q_vy (0.2→0.4); leave EKF as-is.
Re-run the S-trajectory with mixed-μ patches on the floor.
Plot segment-wise RMSE and innovation norms—you should finally see meaningful EKF vs UKF differences.
If you want, I can whip up a tiny odom_cov_scaler.py that subscribes to odom + IMU and republishes odom with curvature/slip-dependent covariances.
Ahh—this dump explains a ton. Two big red flags jump out:
Real ODOM
pose.covariance = all 0
twist.covariance = all 0
IMU
orientation_covariance = all 0
In Robot Localization (and pretty much any KF), a 0 on the diagonal means “perfect, noise-free measurement.” The filter will:
Treat odom pose/twist as ground truth → EKF and UKF will both just follow it ⇒ they look the same.
Become numerically brittle; if anything conflicts (e.g., delayed IMU, a mag gate, a brief inconsistency), the update gains can spike and you can get NaNs.
You already saw this when the UKF blew up: “poorly conditioned…covariances.” Zero (or tiny) R is a classic trigger.
Keep publishing the raw topics, but wrap them and publish a version with sane, nonzero covariances (you were doing this with /model/mmtr/odom_cov_fixed and /imu/cov_fixed—make sure the filters actually subscribe to those, not the raw feeds).
Good, safe starting diagonals (tune later):
Odom (pose) for planar tracking:
markdownpose.cov = diag([0.05, 0.05, 1.0, 1.0, 1.0, 0.03])
x y z roll pitch yaw
Odom (twist):
markdowntwist.cov = diag([0.04, 0.04, 0.01, 0.01, 0.01, 0.015])
vx vy vz vroll vpitch vyaw
IMU (orientation) (if you feed roll/pitch only from IMU0 and yaw from mag):
csharporientation_cov = diag([0.00122, 0.00122, 99999.0])
roll pitch yaw (set huge so IMU yaw is ignored)
…and for mag yaw (your /imu/mag_yaw):
iniorientation_cov = diag([99999.0, 99999.0, 0.02]) # yaw variance ~0.02 rad² to start
Make sure your launch points EKF/UKF to these cov_fixed / mag_yaw topics:
EKF/UKF imu0: /imu/cov_fixed (or your delayed version)
EKF/UKF imu1: /imu/mag_yaw
EKF/UKF odom0: /model/mmtr/odom_cov_fixed
If you see any zero covariances getting into the filter, that’s a bug to squash first.
frame_id: mmtr/odom (custom frame name) and empty child_frame_id: ''.
twist.twist.* are all 0 in that sample (so “real odom” may be a pose only dead-reckoning snapshot, not the actual body twist).
IMU time (sec: 287) is ~31 s after the odom time (sec: 256) in your snippet.
Any of these can degrade or destabilize fusion if not handled:
Ensure frames are consistent with your RL config:
odom_frame: odom (or set it to mmtr/odom if you must—but then use that everywhere).
base_link_frame: base_footprint.
Set child_frame_id on odom to base_footprint (or set odom0_pose_use_child_frame: false, which you already do, but child id still helps consistency).
Feed a twist that actually reflects motion (or don’t fuse twist if your odom doesn’t have it).
Check time sync:
Use /use_sim_time: true (you do).
Keep sensor_timeout generous (e.g., 0.5) when you add IMU delay/jitter.
If you delay IMU, set imu0_nodelay: false.
With zero (or tiny) covariances, your odom becomes de facto ground truth. Both filters then just pass it through with small smoothing → they look identical. It’s not the controller alone doing this; it’s the measurement models asserting near-perfect certainty.
Enforce nonzero covariances on your feeds (use your covariance_override.py or the “cov_fixed” topics you already had).
Verify subscriptions:
EKF/UKF odom0 → /model/mmtr/odom_cov_fixed
EKF/UKF imu0 → /imu/cov_fixed (or delayed)
EKF/UKF imu1 → /imu/mag_yaw
Fix odom frames: give child_frame_id: base_footprint and use a consistent odom_frame (either change the frame_id to odom or set RL odom_frame: mmtr/odom everywhere).
Add sensor_timeout: 0.5 and keep imu0_nodelay: false if you delay IMU.
Once stable, if you want EKF vs UKF to diverge:
Gate mag yaw for 8–12 s during turns, and/or lower mag rate.
Add curvature/slip-dependent covariance inflation on odom.
Increase UKF process noise on vyaw (and vy if used) slightly more than EKF.
If you want, I can give you a tiny odom_cov_scaler.py that subscribes to odom + imu and republishes odom with inflated covariances when turning or when |gyro_z - odom_ωz| is large—great way to inject realism and reveal differences between EKF and UKF.
Great—thanks for the snippet. Your /model/mmtr/odom_cov_fixed looks sane now: non-zero diagonals on pose/twist covariances and child_frame_id: base_footprint. A few practical notes to square this with the “EKF≈UKF” behavior you’re still seeing:
Pose cov diagonals ≈ [0.05, 0.05, 0.05, 1.0, 1.0, 0.03] → reasonable.
Twist cov diagonals ≈ [0.04, 0.04, 0.01, 0.01, 0.01, 0.015] → also fine.
You’ve kept yaw pose out of the odom measurement in the filter configs (good).
Are you fusing odom twist while it’s zero?
In your sample the twist values are all 0. If that’s common (Gazebo odom not providing meaningful body twists), but your config has
vbnetodom0_config: [pos x,y,true, …, vx=true, vy=true, vz=true, …]
then you’re telling the filter to trust zero velocity during motion. That will pin the filter and make EKF and UKF look the same.
If your odom twist isn’t reliable, turn off those three: set vx, vy, vz to false.
Or compute body twist from encoders and publish it properly.
Frame naming consistency.
Your message uses frame_id: mmtr/odom, but both EKF and UKF are configured with odom_frame: odom. That mismatch doesn’t crash RL, but it can make TF/debugging messy and can bite if you use odom0_pose_use_child_frame elsewhere.
Either change the filter params to odom_frame: mmtr/odom, or make your odom publisher use frame_id: odom. Pick one and be consistent.
Sensor timing & IMU delay flags.
If you’re running the delayed IMU path, keep:
imu0_nodelay: false
sensor_timeout: ~0.5 (add this if you haven’t)
Mismatched delay settings are a common source of UKF NaNs.
Innovation/residuals tell you if odom dominates.
Turn on RL diagnostics (you already have print_diagnostics: true). Watch the innovation magnitudes
for odom vs IMU/mag. If odom innovations are tiny while IMU yaw is
frequently gated/inflated, both EKF/UKF will follow odom similarly.
With the same sensors, similar covariances, and mostly near-linear planar motion, the UKF doesn’t get to flex its sigma-point advantages. If odom dominates (especially via position and—if enabled—“zero” twist), both filters will look nearly identical.
Disable unreliable channels: set odom vx,vy,vz=false if they’re not good.
Inflate odom cov during turns/slip: e.g., scale pose/twist cov by a factor when |gyro_z| or curvature is high.
Gate mag yaw in bursts (your gater node) so the IMU yaw carries you briefly; the UKF usually handles this a bit better if you also increase process_noise for vyaw.
Different process noise: Give the UKF slightly larger Q for yaw rate (and lateral velocity if used) than the EKF.
In both EKF & UKF configs:
Set odom0_config velocity row to false,false,false unless your twist is trustworthy.
Align odom_frame to mmtr/odom (or change the publisher to odom).
Add sensor_timeout: 0.5.
Re-run your circle/S tests and re-plot XY error over time and yaw error. You should see:
Slightly larger drift (since you stopped “injecting” zero twist), and
More visible differences between EKF and UKF once mag gating/slip inflation kick in.
If you want, I can give you a 30-line odom_cov_scaler.py that subscribes to odom+IMU and republishes odom with curvature-dependent covariance inflation—that’s a simple, realistic way to expose EKF/UKF differences without touching the controller.